#define IN1 2 //PTO switch
#define IN2 4  // IQAN input
#define LED1 13
#define WIPER_VALUE_PIN A0
int flag = 0;
int pto_state = 0;
int switch_state = 0; // state of the new switch

//variables for DS3502
int min = 0;
float max = 78.5;
int throttle = 0;
int t_inc = 500;
int t_dec = 300;
float wiper_value = 0;
int i = 0;

#include <Adafruit_DS3502.h>
Adafruit_DS3502 ds3502 = Adafruit_DS3502();

void setup(){
    fsm_init();
}
void loop(){
    fsm_update();
}

void (*fsm_state)(void);
bool fsm_enter_state_flag = true;
void fsm_init(){
    fsm_state = &fsm_init_state;
}
void fsm_update(){
    (*fsm_state)();
}
void fsm_init_state (){
    pto_state = digitalRead(IN1);
    switch_state = digitalRead(IN2);  // read the new switch
    if (fsm_enter_state_flag){
        mow_zero();
        pinMode(IN1, INPUT);
        pinMode(IN2, INPUT);  // set the new switch as input
        pinMode(LED1, OUTPUT);
        Serial.begin(115200);
        digitalWrite(LED1, LOW);
        Serial.println("----------------");
        Serial.println("init");
        flag = 0;
    }
    if( pto_state == 0 && switch_state == 0 && flag == 0 ){
        fsm_state = &fsm_idle_state;
        fsm_enter_state_flag = true;
        return;
    }

    if( pto_state == 1 || switch_state == 1 && flag == 0 ){
        fsm_state = &fsm_Wait_reset_state;
        fsm_enter_state_flag = true;
        return;
    }
    fsm_enter_state_flag = false;
}

void fsm_idle_state (){
    pto_state = digitalRead(IN1);
    switch_state = digitalRead(IN2);
    if (fsm_enter_state_flag){
        mow_zero();
        digitalWrite(LED1, pto_state);
        Serial.println("----------------");
        Serial.println("idle");
        flag = 0;
        delay(1000);
    }
    if( pto_state == 1 && switch_state == 0 && flag == 0 ){
        fsm_state = &fsm_Ramp_up_state;
        fsm_enter_state_flag = true;
        return;
    }
    if( switch_state == 1 ){
        fsm_state = &fsm_Wait_reset_state;
        fsm_enter_state_flag = true;
        return;
    }
    fsm_enter_state_flag = false;
}

void fsm_Ramp_up_state (){
    pto_state = digitalRead(IN1);
    switch_state = digitalRead(IN2);
    if (fsm_enter_state_flag){
        digitalWrite(LED1, HIGH);
        Serial.println("----------------");
        Serial.println("ramp up starting");
        ramp_up_cmd();
        Serial.println("ramp up completed");
        flag = 1;
        delay(1000);
    }
    if( pto_state == 1 && switch_state == 0 && flag == 1 ){
        fsm_state = &fsm_Mowing_state;
        fsm_enter_state_flag = true;
        return;
    }
    if( switch_state == 1 ){
        fsm_state = &fsm_Wait_reset_state;
        fsm_enter_state_flag = true;
        return;
    }
    fsm_enter_state_flag = false;
}

void fsm_Ramp_down_state (){
    pto_state = digitalRead(IN1);
    switch_state = digitalRead(IN2);
    if (fsm_enter_state_flag){
        digitalWrite(LED1, HIGH);
        Serial.println("----------------");
        Serial.println("ramp down starting");
        ramp_down_cmd();
        Serial.println("ramp down completed");
        flag = 0;
        delay(1000);
    }
    if( pto_state == 0 && switch_state == 0 && flag == 0 ){
        fsm_state = &fsm_idle_state;
        fsm_enter_state_flag = true;
        return;
    }
    if( switch_state == 1 ){
        fsm_state = &fsm_Wait_reset_state;
        fsm_enter_state_flag = true;
        return;
    }
    fsm_enter_state_flag = false;
}

void fsm_Mowing_state (){
    pto_state = digitalRead(IN1);
    switch_state = digitalRead(IN2);
    flag = 1;
    if (fsm_enter_state_flag){
        digitalWrite(LED1, HIGH);
        Serial.println("----------------");
        Serial.println("mowing state starting");
        mow();
        Serial.println("mowing state completed");
        delay(1000);
    }
    // Check if PTO (IN1) is off first, before checking switch_state
    if( pto_state == 0 && flag == 1 ){
        fsm_state = &fsm_Ramp_down_state;
        fsm_enter_state_flag = true;
        return;
    }
    // This condition will now only be checked if the above condition is false
    if( switch_state == 1 ){
        fsm_state = &fsm_Wait_reset_state;
        fsm_enter_state_flag = true;
        return;
    }
    fsm_enter_state_flag = false;
}

void fsm_Wait_reset_state (){
    pto_state = digitalRead(IN1);
    switch_state = digitalRead(IN2);
    if (fsm_enter_state_flag){
        mow_zero();
        digitalWrite(LED1, LOW);
        Serial.println("----------------");
        Serial.println("wait reset");
        flag = 0;
        delay(1000);
    }
    if( pto_state == 0 && switch_state == 0 && flag == 0 ){
        fsm_state = &fsm_idle_state;
        fsm_enter_state_flag = true;
        return;
    }
    fsm_enter_state_flag = false;
}

void ramp_up_cmd(){
    throttle = map(max, 0, 100, 0, 127);
    // increase throttle from 0 to speed_goal
    i = throttle/5;
    Serial.print("0/5 - ");
    ds3502.setWiper(0);
    wiper_value = analogRead(WIPER_VALUE_PIN);
    wiper_value *= 5.0;
    wiper_value /= 1024;
    Serial.print(wiper_value);
    Serial.println(" V");
    delay(t_inc); // wait for 100ms

    Serial.print("1/5 - ");
    ds3502.setWiper(i);
    wiper_value = analogRead(WIPER_VALUE_PIN);
    wiper_value *= 5.0;
    wiper_value /= 1024;
    Serial.print(wiper_value);
    Serial.println(" V");
    delay(t_inc); // wait for 100ms

    Serial.print("2/5 - ");
    ds3502.setWiper(i*2);
    wiper_value = analogRead(WIPER_VALUE_PIN);
    wiper_value *= 5.0;
    wiper_value /= 1024;
    Serial.print(wiper_value);
    Serial.println(" V");
    delay(t_inc); // wait for 100ms

    Serial.print("3/5 - ");
    ds3502.setWiper(i*3);
    wiper_value = analogRead(WIPER_VALUE_PIN);
    wiper_value *= 5.0;
    wiper_value /= 1024;
    Serial.print(wiper_value);
    Serial.println(" V");
    delay(t_inc); // wait for 100ms

    Serial.print("4/5 - ");
    ds3502.setWiper(i*4);
    wiper_value = analogRead(WIPER_VALUE_PIN);
    wiper_value *= 5.0;
    wiper_value /= 1024;
    Serial.print(wiper_value);
    Serial.println(" V");
    delay(t_inc); // wait for 100ms

    Serial.print("5/5 - ");
    ds3502.setWiper(i*5);
    wiper_value = analogRead(WIPER_VALUE_PIN);
    wiper_value *= 5.0;
    wiper_value /= 1024;
    Serial.print(wiper_value);
    Serial.println(" V");
    delay(1000); // wait for 100ms
}

void ramp_down_cmd(){
      throttle = map(max, 0, 100, 0, 127);
      // increase throttle from 0 to speed_goal
      i = throttle/5;
      Serial.print("5/5 - ");
      ds3502.setWiper(i*5);
      wiper_value = analogRead(WIPER_VALUE_PIN);
      wiper_value *= 5.0;
      wiper_value /= 1024;
      Serial.print(wiper_value);
      Serial.println(" V");
      delay(t_dec); // wait for 100ms

      Serial.print("4/5 - ");
      ds3502.setWiper(i*4);
      wiper_value = analogRead(WIPER_VALUE_PIN);
      wiper_value *= 5.0;
      wiper_value /= 1024;
      Serial.print(wiper_value);
      Serial.println(" V");
      delay(t_dec); // wait for 100ms

      Serial.print("3/5 - ");
      ds3502.setWiper(i*3);
      wiper_value = analogRead(WIPER_VALUE_PIN);
      wiper_value *= 5.0;
      wiper_value /= 1024;
      Serial.print(wiper_value);
      Serial.println(" V");
      delay(t_dec); // wait for 100ms

      Serial.print("2/5 - ");
      ds3502.setWiper(i*2);
      wiper_value = analogRead(WIPER_VALUE_PIN);
      wiper_value *= 5.0;
      wiper_value /= 1024;
      Serial.print(wiper_value);
      Serial.println(" V");
      delay(t_dec); // wait for 100ms

      Serial.print("1/5 - ");
      ds3502.setWiper(i);
      wiper_value = analogRead(WIPER_VALUE_PIN);
      wiper_value *= 5.0;
      wiper_value /= 1024;
      Serial.print(wiper_value);
      Serial.println(" V");
      delay(t_dec); // wait for 100ms

      Serial.print("0/5 - ");
      ds3502.setWiper(0);
      wiper_value = analogRead(WIPER_VALUE_PIN);
      wiper_value *= 5.0;
      wiper_value /= 1024;
      Serial.print(wiper_value);
      Serial.println(" V");
      delay(t_dec); // wait for 100ms
}

void mow(){
  Serial.print("5/5 - ");
  ds3502.setWiper(i*5);
  wiper_value = analogRead(WIPER_VALUE_PIN);
  wiper_value *= 5.0;
  wiper_value /= 1024;
  Serial.print(wiper_value);
  Serial.println(" V");
}

void mow_zero(){
  Serial.print("0/5 - ");
  ds3502.setWiper(0);
  wiper_value = analogRead(WIPER_VALUE_PIN);
  wiper_value *= 5.0;
  wiper_value /= 1024;
  Serial.print(wiper_value);
  Serial.println(" V");
}