#include <Arduino.h>
#include "mavlink.h"
#include <vector>
using namespace std;

bool mission_count_stop = false;
int cycles_count = 0;
vector<int> mission_items_array;
bool mis_cycle_stop_count = false;
int token_count_for_mis_prev =0;
int token_count_for_mis = 0;
bool stop_count_please = false;
int count_items_array, count_items_to_compare = 0;
unsigned int mission_items_packet;
unsigned int counted_mis_items;
uint8_t _system_id = 255; // id of computer which is sending the command (ground control software has id of 255) 
uint8_t _component_id = 0; // seems like it can be any # except the number of what Pixhawk sys_id is 
uint8_t _target_system = 1; // Id # of Pixhawk (should be 1) 
uint8_t _target_component = 0;

void MIS_REQ_LIST_2() {

  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];


  mavlink_msg_mission_request_list_pack(_system_id, _component_id, &msg, _target_system, _target_component, 	MAV_MISSION_TYPE_MISSION);
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  Serial2.write(buf, len);

}

//Then I immediately decode AND using some sort of filter.

void MIS_COUNT_DECODE() {

  mavlink_message_t msg; 
  mavlink_status_t status;
  mavlink_command_long_t cmd;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 
  while(Serial2.available()) {

    uint8_t c = Serial2.read();
    // uint8_t rlen = Serial2.readBytes(buf, MAVLINK_MAX_PACKET_LEN);

    // Serial3.print(c);
    //Get new message

    if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {

      //Serial3.printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);

      mavlink_mission_count_t packet;
      mavlink_msg_mission_count_decode(&msg, &packet);

      //Serial3.print("\n nmi: "); Serial3.println(packet.count);

      mission_items_packet = packet.count;

      break;
    }
  } 
}

void COUNT_MISSION_ITEMS() {

  if (mission_items_array.size() < 201) {
    mission_items_array.push_back(mission_items_packet);
  }

  //Serial3.print("\n arr size: "); Serial3.println(mission_items_array.size());
  //Serial3.println(mission_items_array[0]);

  if (mission_items_array.size() >= 200) {

    //mission_req_stop = true;
    //std::vector<int> vec = mission_items_array;
    //
 //
    //std::vector<int> indices = findItems(vec, target_mis_cnt);
//
    //for (auto &e: indices) {
    //    Serial3.print("\n indices: "); Serial3.println(e);
    //}

    while ((count_items_array != 201) && (stop_count_please == false)) {

      //if (mission_items_array[count_items_to_compare]) {
      //    count_items_to_compare = count_items_to_compare + 1;
      //}

      for (int i = 0; i < 200; i++) {
        if (mission_items_array[count_items_array] == mission_items_array[i]) {
          token_count_for_mis = token_count_for_mis + 1;
        }

        //Serial3.print("\n Cur Num: "); Serial3.println(mission_items_array[count_items_array]);
        //Serial3.print("\n Seq: "); Serial3.println(mission_items_array[i]);
        
      }


      if (token_count_for_mis > token_count_for_mis_prev) {
        token_count_for_mis_prev = token_count_for_mis;
      }

      if(token_count_for_mis_prev > 80) {
        counted_mis_items = mission_items_array[cycles_count];
        mis_cycle_stop_count = true;
        Serial3.print("\n Got Seq: "); Serial3.println(counted_mis_items);
        Serial3.print("\n How Many: "); Serial3.println(token_count_for_mis_prev);
        stop_count_please = true;
        mission_count_stop = true;
      }

      count_items_array = count_items_array + 1;
      token_count_for_mis = 0;
      count_items_to_compare = 0;

      //Serial3.print("\n Cur Number: "); Serial3.println(count_items_array);
      //Serial3.print("\n Cur Tokens: "); Serial3.println(token_count_for_mis_prev);

      //Serial3.print("\n CIA: "); Serial3.print(count_items_array);

      //if (count_items_to_compare >= 201) {
      //  
      //}

      if (mis_cycle_stop_count == false) {
        cycles_count = cycles_count + 1;
      }

      if (count_items_array >= 200) {
        mission_count_stop = true;
        stop_count_please = true;
        Serial3.print("\n Count Gave Error: "); Serial3.println(mission_items_array[cycles_count]);

        for (int i = 0; i < 200; i++) {
          Serial3.print( mission_items_array[i] + ",");
        }
        
      }
      
    }
    //target_mis_cnt = target_mis_cnt + 1;

  }
}