// Define pin numbers
#define SWITCH_PIN 2
#define LED_PIN 13
#define WIPER_VALUE_PIN A0
#define SS_RELAY 3  // Solid State Relay control pin

// State variable and function pointer for state management
void (*fsm_state)(void);
bool fsm_enter_state_flag = true;

// Variables for DS3502
float max_v = 86.5;//81.995
int throttle = 0;
int t_inc = 500;
int t_dec = 300;
float wiper_value = 0;
int i = 0;

// Cycle control variables
const int cycle_total = 5;        // Number of cycles to run
const unsigned long braking_wait = 3000; // ms to wait between cycles

// Include the Adafruit DS3502 library
#include <Adafruit_DS3502.h>
Adafruit_DS3502 ds3502 = Adafruit_DS3502();

// Variable to track the last state of the switch
bool lastSwitchState = HIGH; // Assume switch is not pressed at start

// Variables for cycle counting and timing inside fsm_cycle_test
int current_cycle = 0;
unsigned long cycle_start_time = 0;
enum CyclePhase { RELAY_ON, RAMP_UP, MOWING, RAMP_DOWN, MOW_ZERO, RELAY_OFF, BRAKING_WAIT };
CyclePhase cycle_phase = RELAY_ON;

// Flag to print mowing message only once per mowing phase
bool mowing_msg_printed = false;

void fsm_init(){
    pinMode(SWITCH_PIN, INPUT); // Initialize switch pin as input
    pinMode(LED_PIN, OUTPUT); // Initialize LED pin as output
    pinMode(SS_RELAY, OUTPUT); // Initialize relay pin as output
    digitalWrite(SS_RELAY, HIGH); // Start with relay off (assuming HIGH is off)
    fsm_state = &fsm_init_state;
}

void fsm_update(){
    (*fsm_state)(); // Call the current state function
}

void fsm_init_state (){
    if (fsm_enter_state_flag){
        Serial.begin(115200);
        delay(500);
        ds3502.begin();
        digitalWrite(LED_PIN, LOW);
        mow_zero();
        Serial.println("----------          init");
        fsm_enter_state_flag = false;
        if(digitalRead(SWITCH_PIN) == LOW){
            fsm_state = &fsm_cycle_test_state;  // Enter cycle test if switch pressed
        } else {
            fsm_state = &fsm_idle_state;
        }
        fsm_enter_state_flag = true;
    }
}

void fsm_idle_state (){
    if (fsm_enter_state_flag){
        Serial.println("----------          idle");
        fsm_enter_state_flag = false;
    }
    bool currentSwitchState = digitalRead(SWITCH_PIN);
    if(lastSwitchState == HIGH && currentSwitchState == LOW){
        fsm_state = &fsm_cycle_test_state;
        fsm_enter_state_flag = true;
    }
    lastSwitchState = currentSwitchState;
}

void fsm_cycle_test_state(){
    if (fsm_enter_state_flag){
        Serial.println("----------          cycle test start");
        current_cycle = 0;
        cycle_phase = RELAY_ON;
        cycle_start_time = millis();
        fsm_enter_state_flag = false;
        digitalWrite(LED_PIN, HIGH);
        mowing_msg_printed = false;  // Reset mowing message flag
    }

    bool switch_pressed = (digitalRead(SWITCH_PIN) == LOW);

    // Continue cycling only if cycle count not met
    if (current_cycle < cycle_total) {
        unsigned long now = millis();

        switch(cycle_phase) {
            case RELAY_ON:
                Serial.println("Cycle Phase: RELAY_ON");
                digitalWrite(SS_RELAY, LOW);  // Activate relay
                cycle_start_time = now;
                cycle_phase = RAMP_UP;
                break;

            case RAMP_UP:
                if (now - cycle_start_time >= 100) {  // wait 100ms
                    Serial.println("Cycle Phase: RAMP_UP");
                    ramp_up_cmd();
                    cycle_start_time = now;
                    cycle_phase = MOWING;
                    mowing_msg_printed = false;  // Reset flag for mowing phase
                }
                break;

            case MOWING:
                if (!mowing_msg_printed) {
                    Serial.println("----------          mowing at 100%");
                    mowing_msg_printed = true;
                }
                if (cycle_start_time == 0) cycle_start_time = now; // Safety init
                if (now - cycle_start_time < 10000) { // mow for 10 seconds
                    mow();
                } else {
                    Serial.println("Cycle Phase: RAMP_DOWN");
                    ramp_down_cmd();
                    cycle_phase = RAMP_DOWN;
                    cycle_start_time = now;
                }
                break;

            case RAMP_DOWN:
                // ramp_down_cmd() is blocking, so immediately move on
                Serial.println("Cycle Phase: MOW_ZERO");
                mow_zero();
                cycle_phase = MOW_ZERO;
                cycle_start_time = now;
                break;

            case MOW_ZERO:
                Serial.println("Cycle Phase: RELAY_OFF");
                digitalWrite(SS_RELAY, HIGH);  // Deactivate relay
                cycle_phase = RELAY_OFF;
                cycle_start_time = now;
                break;

            case RELAY_OFF:
                if (now - cycle_start_time >= braking_wait) {
                    current_cycle++;
                    Serial.print("Cycle ");
                    Serial.print(current_cycle);
                    Serial.println(" completed");
                    cycle_phase = RELAY_ON;
                    cycle_start_time = now;
                }
                break;

            default:
                cycle_phase = RELAY_ON;
                cycle_start_time = now;
                break;
        }
    } else {
        // Cycle count completed, stop cycling
        Serial.println("----------          cycle test completed");

        digitalWrite(LED_PIN, LOW);
        mow_zero();
        digitalWrite(SS_RELAY, HIGH);  // Ensure relay off

        if (switch_pressed) {
            // If switch still pressed, go to wait reset state
            fsm_state = &fsm_wait_reset_state;
        } else {
            // Otherwise, go to idle
            fsm_state = &fsm_idle_state;
        }
        fsm_enter_state_flag = true;
    }
}

void fsm_ramp_up_state (){
    if (fsm_enter_state_flag){
        digitalWrite(LED_PIN, HIGH);
        Serial.println("----------          ramp up starting");
        ramp_up_cmd();
        Serial.println("----------          ramp up completed");
        fsm_state = &fsm_mowing_state;
        fsm_enter_state_flag = true;
    }
}

void fsm_ramp_down_state (){
    if (fsm_enter_state_flag){
        digitalWrite(LED_PIN, LOW);
        Serial.println("----------          ramp down starting");
        ramp_down_cmd();
        delay(1000); // 1 second delay as specified
        fsm_state = &fsm_not_mowing_state;
        fsm_enter_state_flag = true;
    }
}

void fsm_mowing_state (){
    if (fsm_enter_state_flag){
        Serial.println("----------          mowing");
        mow();
        fsm_enter_state_flag = false;
    }
    if(digitalRead(SWITCH_PIN) == HIGH){
        fsm_state = &fsm_ramp_down_state;
        fsm_enter_state_flag = true;
    }
}

void fsm_wait_reset_state (){
    if (fsm_enter_state_flag){
        digitalWrite(LED_PIN, LOW);
        mow_zero();
        Serial.println("----------          wait reset");
        fsm_enter_state_flag = false;
    }
    // Directly check the SWITCH_PIN state for transition
    if(digitalRead(SWITCH_PIN) == HIGH){
        fsm_state = &fsm_not_mowing_state;
        fsm_enter_state_flag = true;
    }
}

void fsm_not_mowing_state(){
    if(fsm_enter_state_flag){
        Serial.println("----------          not mowing");
        mow_zero();
        delay(1000); // 1 second delay as specified
        fsm_state = &fsm_idle_state;
        fsm_enter_state_flag = false; // Prevent this block from running again until state changes
    }
}

void setup(){
    fsm_init();
}

void loop(){
    fsm_update();
}

void wiper_value_func(){
    wiper_value = analogRead(WIPER_VALUE_PIN);
    wiper_value *= 5.0;
    wiper_value /= 1024;
    Serial.print(wiper_value);
    Serial.println(" V");
}

void ramp_up_cmd(){
    throttle = map(max_v, 0, 100, 0, 127);
    i = throttle / 5;
    for (int j = 0; j <= 5; ++j) {
        Serial.print("-----");
        Serial.print(j * 20);
        Serial.print("% to wiper -->  ");
        ds3502.setWiper(i * j);
        wiper_value_func();
        delay(t_inc);
    }
}

void ramp_down_cmd(){
    throttle = map(max_v, 0, 100, 0, 127);
    i = throttle / 5;
    for (int j = 5; j >= 0; --j) {
        Serial.print("-----");
        Serial.print(j * 20);
        Serial.print("% to wiper -->  ");
        ds3502.setWiper(i * j);
        wiper_value_func();
        delay(t_dec);
    }
}

void mow(){
    ds3502.setWiper(i * 5);
    wiper_value_func();
}

void mow_zero(){
    Serial.println("----------          blades off");
    ds3502.setWiper(0);
    wiper_value_func();
}

void test_ds3502(){
  Serial.print("Wiper voltage with wiper set to 0: ");
  ds3502.setWiper(0);
  float wiper_value = analogRead(WIPER_VALUE_PIN);
  wiper_value *= 5.0;
  wiper_value /= 1024;
  Serial.print(wiper_value);
  Serial.println(" V");

  Serial.println();
  delay(1000);

  Serial.print("Wiper voltage with wiper set to 63: ");
  ds3502.setWiper(63);
  wiper_value = analogRead(WIPER_VALUE_PIN);
  wiper_value *= 5.0;
  wiper_value /= 1024;
  Serial.print(wiper_value);
  Serial.println(" V");

  Serial.println();
  delay(1000);

  Serial.print("Wiper voltage with wiper set to 127: ");
  ds3502.setWiper(127);
  wiper_value = analogRead(WIPER_VALUE_PIN);
  wiper_value *= 5.0;
  wiper_value /= 1024;
  Serial.print(wiper_value);
  Serial.println(" V");

  Serial.println();
  delay(1000);
}