#include <CAN.h>

//STEPPERS
  // FLAPERON R   -- STEPPER

  // FLAPERON L   -- STEPPER 1
  
  // STABILATOR R -- STEPPER 2

  // STABILATOR L -- STEPPER 3 
  
  // DIRECTIE     -- STEPPER 4
    
  // LEADING FLAP -- STEPPER 5
    

  //Int semnale/actuatoare

    int semnal_mansa_ruliu = 0;

    int semnal_mansa_tangaj = 0;

    int semnal_palonier = 0;

    int actuator_flaperon_R = 0;

    int actuator_flaperon_L = 0;

    int actuator_stabilator_R = 0;

    int actuator_stabilator_L = 0;

    int actuator_directie = 0;

    int leading_edge_flap = 0;

    int actuator_leading_edge_flap = 0;

//IRS
  int viteza = 0;

  int altimetru = 0;

  int unghi_de_atac = 0;

//PILOT AUTOMAT
  int ap_pitch_alt_hold = 0;
  int ap_pitch_att_hold = 0;
  int ap_roll_att_hold = 0;
  int ap_roll_hdg_sel = 0;
  

//TRIMMER MANSA
  int trimmer_tangaj_fata = 0;
  int trimmer_ruliu_dreapta = 0;
  int trimmer_tangaj_spate = 0;
  int trimmer_ruliu_stanga = 0;

//STABILATOR VIRAJ DREAPTA
  int stabilator_R_viraj_dreapta = 0;
  int stabilator_L_viraj_dreapta = 0;
//STABILATOR VIRAJ STANGA
  int stabilator_R_viraj_stanga = 0;
  int stabilator_L_viraj_stanga = 0;

//STABILATOR LA STABILIZARE 
  int stabilator_R_stabilizare = 0;
  int stabilator_L_stabilizare = 0;

//FLAPS ANGLE
  int flaps_0 = 0;
  int flaps_2 = 0;
  int flaps_actionat = 0;
  int leading_edge_2 = 0;
  int leading_edge_15 = 0;
  int leading_edge_25 = 0;

//STARI INITIALE MOTOARE
  int previous = 0;//   FLAPERON R
  int previous1 = 0;//  FLAPERON L
  int previous2 = 0;//  STABILATOR R
  int previous3 = 0;//  STABILATOR L
  int previous4 = 0;//  DIRECTIE
  int previous5 = 0;//  LEADING EDGE FLAP -2
  int previous6 = 0;//  FLAPS 20
  int previous7 = 0;//  FLAPS -2
  int previous8 = 0;//  LEADING EDGE FLAP 15
  int previous9 = 0;//  LEADING EDGE FLAP 25
  int previous10 = 0;// STABILATOR R VIRAJ DREAPTA
  int previous11 = 0;// STABILATOR L VIRAJ DREAPTA
  int previous12 = 0;// STABILATOR R VIRAJ STANGA
  int previous13 = 0;// STABILATOR L VIRAJ STANGA
  int previous14 = 0;// STABILATOR R STABILIZARE UNGHI DE ATAC
  int previous15 = 0;// STABILATOR L STABILIZARE UNGHI DE ATAC

void setup() {
  //UNGHIURI SUPRAFETE DE COMANDA
  flaps_0 = 0;
  flaps_2 = -2;  
  flaps_actionat = 20;
  leading_edge_2 = -2;
  leading_edge_15 = 15;
  leading_edge_25 = 25;
  stabilator_R_viraj_stanga = 90;
  stabilator_L_viraj_stanga = -90;
  stabilator_R_viraj_dreapta = -90;
  stabilator_L_viraj_dreapta = 90;
  stabilator_R_stabilizare = 20;
  stabilator_L_stabilizare = 20;

}

void loop() {
  //CAN BUS
    
  //BUTOANE MANSA & PILOT AUTOMAT
    ap_roll_hdg_sel = digitalRead(9);
    ap_roll_att_hold = digitalRead(8);
    ap_pitch_att_hold = digitalRead(7);
    ap_pitch_alt_hold = digitalRead(6);
    /// trimmer_tangaj_fata = digitalRead(8);
    ///trimmer_ruliu_dreapta = digitalRead(9);
    ///trimmer_tangaj_spate = digitalRead(10);
    ///trimmer_ruliu_stanga = digitalRead(11);
    ///ap_overright = digitalRead(12);

  //Flaps taxi
   if (viteza <= 10 && altimetru < 20) {
    
      CAN.beginPacket(0x12);
   CAN.write('stepper5.move((leading_edge_2 - previous5));
      previous5 = leading_edge_2;');
    CAN.endPacket();
    } else {
         CAN.beginPacket(0x13);
    CAN.write('stepper5.move((leading_edge_2 - previous5));
    previous5 = leading_edge_2;');
    CAN.endPacket();
    }


  //Flaps decolare/aterizare
  if (viteza <= 40 && unghi_de_atac < 24.5) {
    if (altimetru > 20 && altimetru < 60) {
       CAN.beginPacket(0x14);
  CAN.write('stepper5.move((leading_edge_15 - previous8));
                previous8 = leading_edge_15;');
    CAN.endPacket();
  } else {
    CAN.beginPacket(0x15);
   CAN.write('stepper5.move((leading_edge_2 - previous5));
              previous5 = leading_edge_2;');
    CAN.endPacket();
  }}

  // Leading edge unghi de atac mare
  if (viteza >= 60 && altimetru > 60) {
    if (unghi_de_atac > 10 && unghi_de_atac < 24.5) {
      CAN.beginPacket(0x16);
   CAN.write('stepper5.move((leading_edge_25 - previous9));
        previous9 = leading_edge_25;');
    CAN.endPacket();
    }
  } else {
    CAN.beginPacket(0x17);
   CAN.write('stepper5.move((leading_edge_2 - previous5));
      previous5 = leading_edge_2;');
    CAN.endPacket();
  }

  // Stabilator viraj dreapta
  if (semnal_mansa_ruliu > 980 && semnal_mansa_tangaj > 980) {
    if (unghi_de_atac > 24.5 && Viteza >= 60) {
      CAN.beginPacket(0x18);
   CAN.write('
        stepper2.move((stabilator_R_viraj_dreapta - previous10));
            previous10 = stabilator_R_viraj_dreapta;

        stepper3.move((stabilator_L_viraj_dreapta - previous11));
            previous11 = stabilator_L_viraj_dreapta;');
    CAN.endPacket();
    }}
  // Stabilator viraj stanga
  if (semnal_mansa_ruliu > 44 && semnal_mansa_tangaj > 44) {
    if (unghi_de_atac > 24.5 && viteza >= 60) {
      CAN.beginPacket(0x19);
   CAN.write('
        stepper2.move((stabilator_R_viraj_stanga - previous12));
            previous12 = stabilator_R_viraj_stanga;

        stepper3.move((stabilator_L_viraj_stanga - previous13));
            previous13 = stabilator_L_viraj_stanga;
    CAN.endPacket();
      
    }}

  //STABILIZARE UNGHI DE ATAC
 if (unghi_de_atac > 24.5) {

    CAN.beginPacket(0x20);
   CAN.write('
        stepper.move((stabilator_R_stabilizare - previous14));
          previous14 = stabilator_R_stabilizare;

        stepper1.move((stabilator_L_stabilizare - previous15));
          previous15 = stabilator_L_stabilizare;')
    CAN.endPacket();
 }   
  

 
  //Trimmer tanagaj

        
           if (trimmer_tangaj_fata == 1) {

              CAN.beginPacket(0x21);
                CAN.write('
                    previous2=map(trimmer_tangaj_fata,0,1023,-90,90);
                    previous3=map(trimmer_tangaj_fata,0,1023,-90,90);')
              CAN.endPacket();
           }
            if (trimmer_tangaj_spate == 1) {
              CAN.beginPacket(0x22);
                CAN.write('
                    previous2=map(trimmer_tangaj_spate,0,1023,-90,90);
                    previous3=map(trimmer_tangaj_spate,0,1023,-90,90);')
              CAN.endPacket();
              }
  //Trimmer ruliu

            if (trimmer_ruliu_stanga == 1) {
              CAN.beginPacket(0x23);
                CAN.write('
                    previous=map(trimmer_ruliu_stanga,0,1023,-90,90);
                    previous1=map(trimmer_ruliu_stanga,0,1023,90,-90);')
              CAN.endPacket();
           }
            if (trimmer_ruliu_dreapta == 1) {
              CAN.beginPacket(0x23);
                CAN.write('
                    previous=map(trimmer_ruliu_dreapta,0,1023,90,-90);
                    previous1=map(trimmer_ruliu_dreapta,0,1023,-90,90);')
              CAN.endPacket();
           }



}