#include <Servo.h>

const int lidservoPin = 6;
int mainswitch = 5;
int mainswitchstate = HIGH;
int switchstate;
int currentlidservopos = 0;
int targetlidservopos = 0;

int servoangle = 0;

int targetachieved = 0;

int alreadymode = 0;
int mode = 0;    //starts in mode 0
unsigned long timehigh;
unsigned long timelow;
unsigned long timeincurrentmode;
unsigned long timedelay = 10000;
unsigned long resettimer = 5000;

int remembermode = -1;
int alreadyreset;

int speed1 = 1;
int speed2 = 2;
int speed5 = 5;
int speed10 = 10;

Servo lidservo;


void setup() {
  pinMode(mainswitch, INPUT_PULLUP);
  lidservo.attach(lidservoPin); // Attache the arm to the pin 2
  Serial.begin(9600);
  
}

void loop() {

  digitalRead(mainswitch);
  mainswitchstate = digitalRead(mainswitch);
  timeincurrentmode = timelow - timehigh;

//read button and save times only!
  if(mainswitchstate == LOW) {   // trigger
    timelow = millis();          // correct
    if(switchstate == LOW){      // correct - inverting logic
      switchstate = HIGH;
    }
  }

  if(mainswitchstate == HIGH){   // release
    timehigh = millis();         // correct
    mode = 0;                    // correct - keep at mode 0 when high
    if(switchstate == HIGH){     // correct - inverting logic - keep the if
      switchstate = LOW;
      alreadyreset = LOW;        //enable resetting timer 
    }
  }

// timer reset code only! resets to mode 0 after time
  if (mainswitchstate == HIGH) {                //must be high to reset
    if(timehigh - timelow > resettimer){        //check within reset period - resettimer
      if(alreadyreset == LOW){                  // only reset if not done before
        alreadyreset = HIGH;                    // kill yourself
        Serial.println("time expired, settig mode to 0");
        mode = 0;                               // dont worry, only setting it to mode 0 once
        alreadymode = -1;                       // keep mode and already in mode together to engage mode 0
      }
    }    
  }


Serial.print("time high - low: ");
Serial.println(timehigh - timelow);
//if time expired,set to mode 1 if in mode 0
  if (mainswitchstate == LOW && mode == 0) { 
    if(timehigh - timelow > resettimer){
      mode = 1;
    }
    else{
      mode = 2;
    }
  }




//mode 0  
  if(mode == 0 && alreadymode !=0){
    Serial.println(mode);
    alreadymode = 0;
    lidservo.write(0);
    currentlidservopos = 0;
    targetlidservopos = 0; 
  }

//mode 1  
  if(mode == 1){
    Serial.println(mode);
    alreadymode = 1;
    targetlidservopos = 180;

    if (currentlidservopos != targetlidservopos && mainswitchstate == LOW) {
      currentlidservopos = currentlidservopos + speed1;
      delay(50);
      lidservo.write(currentlidservopos);
    }
  }

//mode 2
  if(mode == 2){
    Serial.println(mode);
    alreadymode = 2;
    targetlidservopos = 90; 
    if(targetlidservopos != currentlidservopos){
      currentlidservopos = currentlidservopos + speed1;
      delay(50);
      lidservo.write(currentlidservopos);
    }
    if(targetlidservopos == currentlidservopos){
      delay (1000);
      lidservo.write(180);
    }     
  }

//mode 3
  if(mode == 3){
    Serial.println(mode);
    alreadymode = 3;
    targetlidservopos = 60; 
    if(targetlidservopos != currentlidservopos){
      currentlidservopos = currentlidservopos + speed1;
      delay(50);
      lidservo.write(currentlidservopos);
    }
    if(targetlidservopos == currentlidservopos){
      delay (1000);
      lidservo.write(180);
    }     
  }

//mode 4
  if(mode == 4){
    Serial.println(mode);
    alreadymode = 3;

    if(targetachieved == 0){
      targetlidservopos = 120;

      if(currentlidservopos < targetlidservopos){
        currentlidservopos = currentlidservopos + speed1;
        lidservo.write(currentlidservopos);
        delay(50);
      }
     
      else if(currentlidservopos > targetlidservopos){
        currentlidservopos = currentlidservopos - speed1;
        lidservo.write(currentlidservopos);
        delay(50);
      }
    }    

    if((currentlidservopos - targetlidservopos) == 0 && targetachieved == 0){
      targetachieved = 1;
      Serial.print("target achived: ");
      Serial.println(targetachieved);      
    }

    if(targetachieved == 1){
      targetlidservopos = 60;

      while(currentlidservopos < targetlidservopos  && mainswitchstate == LOW){
        currentlidservopos = currentlidservopos + speed1;
        lidservo.write(currentlidservopos);
        delay(10);
        
      }
     
      while(currentlidservopos > targetlidservopos && mainswitchstate == LOW){
        currentlidservopos = currentlidservopos - speed1;
        lidservo.write(currentlidservopos);
        delay(10);
      }
    }    

    if(currentlidservopos - targetlidservopos == 0 && targetachieved == 1){
      targetachieved = 2;
      Serial.print("target achived: ");
      Serial.println(targetachieved);
    }

    if(targetachieved == 2){
      delay(2000);
      targetlidservopos = 180;

      while(currentlidservopos < targetlidservopos && mainswitchstate == LOW){
        currentlidservopos = currentlidservopos + speed1;
        lidservo.write(currentlidservopos);
        //delay(10);
        
      }
     
      while(currentlidservopos > targetlidservopos && mainswitchstate == LOW){
        currentlidservopos = currentlidservopos - speed1;
        lidservo.write(currentlidservopos);
        //delay(10);
      }
    }    

    if(currentlidservopos - targetlidservopos == 0 && targetachieved == 2){
      targetachieved = 3;
      Serial.print("target achived: ");
      Serial.println(targetachieved);
    }

  }


}