#include <SoftwareSerial.h>
#include <Servo.h>

#define rxPin 2
#define txPin 3
SoftwareSerial altSerial(rxPin, txPin);
Servo myservo;  // create servo object to control a servo

int pos = 0;      // variable to store the servo position
const byte numChars = 32;
char receivedChars[numChars];  // an array to store the received data
boolean newData = false;
unsigned int oldAlt = 9999;
unsigned int newAlt = 0;
unsigned long launchTime;
char rc;

unsigned int flight1[] = {
  0,2,1,1,2,2,3,2,1,2,4,5,9,14,20,25,34,45,54,62,67,70,74,81,87,96,105,113,122,129,140,149,159,167,177,187,197,208,
  218,230,241,252,263,275,286,297,309,321,333,345,357,370,381,394,406,419,432,446,459,473,484,495,505,514,522,530,
  540,547,554,564,574,583,589,597,604,612,619,625,633,640,649,654,659,665,669,675,682,689,693,699,705,710,717,723,
  729,735,739,744,748,755,759,765,770,775,780,783,788,793,796,800,806,809,813,817,821,825,829,833,836,839,843,846,
  849,852,855,860,863,866,868,871,873,876,877,879,883,885,888,890,892,894,896,898,900,901,904,905,907,908,910,910,
  912,913,915,916,916,917,920,921,921,922,923,923,923,924,924,926,926,926,927,928,929,927,928,929,929,930,930,930,
  930,929,927,928,929,929,929,929,929,928,930,932,929,926,925,922,921,918,917,916,916,915,914,915,917,917,917,915,
  912,911,910,907,902,904,904,901,900,897,895,895,895,892,894,892,888,889,888,885,884,886,879,881,878
};

void setup() {
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  altSerial.begin(9600);
  Serial.begin(9600);
  Serial.println("<Arduino is ready>");
}

void loop() {
  recvWithEndMarker();
  showNewData();
  if (newAlt == 505) {
    Serial.println("alt = 505");
    Brake(flight1);
  }
}

void recvWithEndMarker() {
  static byte ndx = 0;
  char endMarker = '\n';

  while (altSerial.available() > 0 && newData == false) {
    Serial.println("servo on");
    rc = altSerial.read();

    if (rc != endMarker) {
      receivedChars[ndx] = rc;
      ndx++;
      if (ndx >= numChars) {
        ndx = numChars - 1;
      }
    } else {
      receivedChars[ndx] = '\0'; // terminate the string
      ndx = 0;
      newData = true;
    }
  }
}

void showNewData() {
  if (newData == true) {
    newAlt = atoi(receivedChars);
    Serial.println(newAlt);
    newData = false;
  }
}

void Brake(unsigned int FlightData[]) {
  for (int index = 0; index <= sizeof(FlightData)/sizeof(int)-1; index++) {
    // You can add your altitude handling logic here
  }
}