/*
  Arduino Wireless Communication Tutorial
      Example 1 - Transmitter Code

  by Dejan Nedelkovski, www.HowToMechatronics.com

  https://howtomechatronics.com/tutorials/arduino/arduino-wireless-communication-nrf24l01-tutorial/?utm_content=cmp-true

  Library: TMRh20/RF24, https://github.com/tmrh20/RF24/
*/

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

RF24 radio(9, 10); // CE, CSN
const byte address[6] = "00001";

// Max size of this struct is 32 bytes - NRF24L01 buffer limit
struct Data_Package {
  int steering = 0;
  int throttle = 0;

  bool s1 = 0;
  bool s2 = 0;

  bool pb1 = 0;
  bool pb2 = 0;

};
Data_Package data; // Create a variable with the above structure

//Analog bemenetek
#define ST A5   // Kormány
#define TH A4   // gázkar
#define POTstTrim A0   // kormány trim
#define POTstRange A1   // kormány range
#define POT3 A2   // 
#define POT4 A3   // 

//digitális bemenetetek
#define SBU1 4   //"stuck button" beragadó nyomógomb
#define SBU2 5

#define BU1 2
#define BU2 3

//kimenetek
#define SB1LED 6
#define SB2LED 7
#define REDLED 8


//változók 
bool sb1 = 0;   //beragadó nyomógomb kimeneti állapot
bool sb2 = 0;

bool b1 = 0;   //nyomógomb kimeneti állapot
bool b2 = 0;

bool sb1_state = 0;     //beragadó myomógomb segédváltozók
bool sb1_laststate = 0;
bool sb2_state = 0;
bool sb2_laststate = 0;

bool b1_state = 0;
bool b2_state = 0;

int ST_out = 0;    //Kormany kimenet hangolt érték  
int TH_out = 0;    //Gázkar kimenet hangolt érték

bool firstbounce = 0;
unsigned long bouncetimer = 0;
int intervall = 50;

void setup() {
  radio.begin();
  radio.openWritingPipe(address);
  radio.setPALevel(RF24_PA_MIN);
  radio.stopListening();
  Serial.begin(9600);
  //Analog bemenetek
  pinMode(ST, INPUT);
  pinMode(TH, INPUT);
  pinMode(POTstTrim, INPUT);
  pinMode(POTstRange, INPUT);
  pinMode(POT3, INPUT);
  pinMode(POT4, INPUT);
  //Digitális bemenetek
  pinMode(SBU1, INPUT_PULLUP);
  pinMode(SBU2, INPUT_PULLUP);
  pinMode(BU1, INPUT_PULLUP);
  pinMode(BU2, INPUT_PULLUP);
  //kimenetek
  pinMode(SB1LED, OUTPUT);
  pinMode(SB2LED, OUTPUT);
  pinMode(REDLED, OUTPUT);
  digitalWrite(SB1LED, HIGH);

  digitalWrite(SB1LED, LOW);
  digitalWrite(SB2LED, LOW);
  digitalWrite(REDLED, HIGH);
  delay(1000);
  digitalWrite(REDLED, LOW);

}

void loop() {

  digitalWrite(SB1LED, sb1);
  digitalWrite(SB2LED, sb2);
  buttons();
  analogpot();

  // Elküldött adatok frissítése
  data.steering = ST_out;
  data.throttle = TH_out;

  data.s1 = sb1;
  data.s2 = sb2;

  data.pb1 = b1;
  data.pb2 = b2;

  //frissített adatok küldése
  radio.write(&data, sizeof(Data_Package));
  //delay(1);
  Serial.print("ST_out: "); Serial.println(data.steering); //Serial.print("  TH_out: "); Serial.println(data.throttle);
  //Serial.print("  b1 "); Serial.print(data.pb1); Serial.print("  b2 "); Serial.println(data.pb2);

}
void buttons(){
 //Sticky button 1 -bal
 if(sb1_state == 0 and digitalRead(SBU1) == LOW and firstbounce == 0){
   firstbounce =1;
   bouncetimer = millis();
 }
 if(sb1_state == 0 and digitalRead(SBU1) == LOW and firstbounce ==1 and millis() > bouncetimer+intervall){
   sb1_state = 1; 
   firstbounce = 0;
 }
 if(sb1_state == 1 and digitalRead(SBU1) == HIGH and firstbounce == 0){
   firstbounce = 1;
   bouncetimer = millis();
 }
 if(sb1_state == 1 and digitalRead(SBU1) == HIGH and firstbounce == 1 and millis() > bouncetimer+intervall){
   sb1_state = 0;
   firstbounce = 0;
 }
  
 if(sb1_state == 1 and sb1_state != sb1_laststate){
  sb1 = !sb1;
 }
  sb1_laststate = sb1_state;

 //sticky button 2 -jobb
 if(sb2_state == 0 and digitalRead(SBU2) == LOW and firstbounce == 0){
   firstbounce =1;
   bouncetimer = millis();
 }
 if(sb2_state == 0 and digitalRead(SBU2) == LOW and firstbounce ==1 and millis() > bouncetimer+50){
   sb2_state = 1; 
   firstbounce = 0;
 }
 if(sb2_state == 1 and digitalRead(SBU2) == HIGH and firstbounce == 0){
   firstbounce = 1;
   bouncetimer = millis();
 }
 if(sb2_state == 1 and digitalRead(SBU2) == HIGH and firstbounce == 1 and millis() > bouncetimer+50){
   sb2_state = 0;
   firstbounce = 0;
 }
  
 if(sb2_state == 1 and sb2_state != sb2_laststate){
  sb2 = !sb2;
 }
  sb2_laststate = sb2_state;

 //push button 1 
 if(b1_state == 0 and digitalRead(BU1) == LOW and firstbounce == 0){
   firstbounce =1;
   bouncetimer = millis();  
 }
 if(b1_state == 0 and digitalRead(BU1) == LOW and firstbounce == 1 and millis() > bouncetimer+intervall){
    b1_state = 1; 
    firstbounce = 0;
  }
 if(b1_state == 1 and digitalRead(BU1) == HIGH and firstbounce == 0){
   firstbounce = 1;
   bouncetimer = millis();
  }
 if(b1_state == 1 and digitalRead(BU1) == HIGH and firstbounce == 1 and millis() > bouncetimer+intervall){
   b1_state = 0;
   firstbounce = 0;
  }
  b1 = b1_state;

  //push button 2
  if(b2_state == 0 and digitalRead(BU2) == LOW and firstbounce == 0){
   firstbounce =1;
   bouncetimer = millis();  
 }
 if(b2_state == 0 and digitalRead(BU2) == LOW and firstbounce == 1 and millis() > bouncetimer+intervall){
    b2_state = 1; 
    firstbounce = 0;
  }
 if(b2_state == 1 and digitalRead(BU2) == HIGH and firstbounce == 0){
   firstbounce = 1;
   bouncetimer = millis();
  }
 if(b2_state == 1 and digitalRead(BU2) == HIGH and firstbounce == 1 and millis() > bouncetimer+intervall){
   b2_state = 0;
   firstbounce = 0;
  }
  b2 = b2_state;
}
void analogpot(){
  //kormány hangolás
  int STraw = map(analogRead(ST), 0, 1023, -500, 500);

  //TRIM
  int st_trim = map(analogRead(POTstTrim), 0, 1023, -50, 50);

  int minValue = 1000 + st_trim *5;
  int maxValue = 2000 + st_trim *5;

  //RANGE

  int st_range = map(analogRead(POTstRange), 0, 1023, 0, 150);

  float SteeringRange = STraw * (st_range / 100.0);

  ST_out = map(SteeringRange, -500.0, 500.0, minValue, maxValue);
  

  /*
  int ST_trim_min = map(POT1Read, 0, 1023, 0, -500);   //ST trim
  int ST_trim_max = map(POT1Read, 0, 1023, 0, 500);

  int ST_center = map(analogRead(POT2), 0, 1023, -100, 100 );  //ST ST_center

  if(ST_center > 0){
    ST_raw = map(analogRead(ST), 0, 1023, ST_trim_min + ST_center, ST_trim_max - ST_center) + ST_center;
  }else{
    ST_raw = map(analogRead(ST), 0, 1023, ST_trim_min - ST_center, ST_trim_max + ST_center) + ST_center;
  }
  ST_out = ST_raw;          //a hangolt értéket átatdjuk egy globális válltozónak
  
  
  //gáz hangolás
  int TH_raw = 0;

  int TH_trim_min = map(POT3Read, 0, 1023, 0, -500);   //ST trim
  int TH_trim_max = map(POT3Read, 0, 1023, 0, 500);

  int TH_center = map(analogRead(POT4), 0, 1023, -100, 100);  //ST ST_center

  if(TH_center > 0){
    TH_raw = map(analogRead(TH), 0, 1023, TH_trim_min + TH_center, TH_trim_max - TH_center) + TH_center;
  }else{
    TH_raw = map(analogRead(TH), 0, 1023, TH_trim_min - TH_center, TH_trim_max + TH_center) + TH_center;
  }
  TH_out = TH_raw;          //a hangolt értéket átatdjuk egy globális válltozónak
  */
}