void SW_state() {
  //0-boot mode
  //1-test mode don't know

  int a1 = altitudes[0];
  int a2 = altitudes[1];
  int a3 = altitudes[2];
  bool isAscending = (a1 < a2) && (a2 < a3);
  bool isDescending = (a1 > a2) && (a2 > a3);
  if ( (SWstate == 0 || SWstate == 2) && a1 <= 20 && a2 <= 20 && a3 <= 20 ) {
    SWstate = 2; //launch pad
  }

  else if ((SWstate == 2 || SWstate == 3 ) && a1 > 100 && a2 > 100 && a3 > 100 && isAscending) {
    //    Serial.println("contrion this");
    if (a1 < 800 && a2 < 800 && a3 < 800) {
      SWstate = 3; //Ascent
    }
    else if (millis() - start_BLDC >= 7210 || (a1 > 800 && a2 > 800 && a3 > 800) ) {
      //camera should start recording here
      ON_camera();
      BLDC.write(80);
      SWstate = 4; //Deployment
    }

  }
  else if ((SWstate == 2 || SWstate == 3 ) && a1 > 100 && a2 > 100 && a3 > 100 && isDescending) {
    ON_camera();
    BLDC.write(80);
    //      SWstate = 4; //Deployment
    SWstate = 5;
  }

  else if ((SWstate == 4 || SWstate == 5) && isDescending &&  a1 < 800 && a2 < 800 && a3 < 800 ) { //((SWstate == 4 || SWstate == 5) && p2 > p1 && p3 > p2 && a2 < a1 && a3 < a2 && a1 < 800 && a2 < 800 && a3 < 800 )
    if (SWstate == 5 && a3 < 500 && a2 < 500 && a3 < 500) { //(SWstate == 5 && a3 <= 500)
      s.write(0); //servo turns to deploy second parachute
      SWstate = 6; //Aerobreak Release
    }
    else {
      SWstate = 5; //Descent
    }
  }

  else if (SWstate == 6) {
    if (a3 == 0 || (a1 < 10 && a2 < 10 && a3 < 10) ) {//(a1 < 5 && a2 < 5 && a3 < 5 && a1 == a2 && a2 == a3)
      BLDC.write(0);
      SWstate = 7; //Impact
      OFF_camera(); //camera stops recording here
      //buzzer should switch on here
    }
    else {
      SWstate = 6;
    }

  }

  else if (a1 < 0.5 && a2 < 0.5 && a3 < 0.5) {
    SWstate = 0;
  }




  packet += String(SWstate);
  packet += ",";
  //   packet+=">\r";

}