// Import bilbiotek
#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
#include <Servo.h> //Biblioteka odpowiedzialna za serwa
#include <DHT.h>
#define DHT11_PIN 34
DHT dht;
#define trigPin 12
#define echoPin 11

//
// Konfiguracja ustawień sprzętowych radia
//
const int max_payload_size = 32; // ustawienie payloadu
RF24 radio(53,48); // Uruchomienie nrfa na pinach 53 i 48

// Ustawienia komunikacji radia
const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL }; // ustawienie radio pipe dla 2 node

// Zmienne odbierające i wysyłające
char receive_payload[max_payload_size+1]; // +1 aby umożłiwić terminowanie wartośći NULL
char sending_payload[3];
// Zmienne silnika
int PWMab = 0; // Zmienna pomocnicza okreslajaca predkosc wynikajaca z triggera gamepada
int PWMa = 0; // Kanał lewy mostka
int PWMb = 0; // Kanał prawy mostka
// Zmienne stanu mostka H
int GoForward =0;
int GoBackward = 0;
int Stop = 1;
int TurnLeft = 0;
int TurnRight = 0;
// Zmienne pinów mostka H
int IN1v = 0;
int IN2v = 0;
int IN3v = 0;
int IN4v = 0;

// Porty arduino
int rightMotorPin = 10;
int leftMotorPin = 9;

int IN1p = 2; // Piny logiczne mostka H IN1-IN4
int IN2p = 3;
int IN3p = 4;
int IN4p = 5;

int Serwo1p = 8; // Piny serwomechanizmow manipulatora
int Serwo2p = 11;
int Serwo3p = 12;
int Serwo4p = 13;

int SerwoPosition[4] = {30, 30, 30, 30}; // Startowa pozycja seromechanizmów

int ChosenSerwo = 0; // zmienna pomocnicza do wyboru serwa z pada


Servo servos[4]; // Stworzenie obiektow serwa

void SerwoControl(unsigned char Button1, unsigned char Button2, unsigned char Button3, unsigned char Button4, unsigned char SerwoPlus, unsigned char SerwoMinus)
{
  if (Button1 == 1)
  {
    ChosenSerwo = 0;
  }

  else if (Button2 == 1)
  {
    ChosenSerwo = 1;
  }
  else if (Button3 == 1)
  {
    ChosenSerwo = 2;
  }
  else if (Button4 == 1)
  {
    ChosenSerwo = 3;
  }
   if (SerwoPlus == 1)
   {
    if (SerwoPosition[ChosenSerwo] < 180)
    {
    SerwoPosition[ChosenSerwo]= SerwoPosition[ChosenSerwo] + 1;
    servos[ChosenSerwo].write(SerwoPosition[ChosenSerwo]);

    }
   }
   else if (SerwoMinus == 1)
   {
    if (SerwoPosition[ChosenSerwo] > 0)
    {
    SerwoPosition[ChosenSerwo]= SerwoPosition[ChosenSerwo] - 1;
    servos[ChosenSerwo].write(SerwoPosition[ChosenSerwo]);

   }
   }


}

void MotorSpeed(unsigned char RightTrigger,unsigned char leftTrigger, char Xaxis)
{
  if ((RightTrigger) > (leftTrigger))
  {
    PWMab =(RightTrigger) - (leftTrigger);
    GoForward = 1;
    GoBackward = 0;
    Stop = 0;
    TurnRight = 0;
     TurnLeft = 0;
  }
  else if ((leftTrigger > (RightTrigger)))
  {
     PWMab = (leftTrigger) - (RightTrigger);
     GoForward = 0;
    GoBackward = 1;
    Stop = 0;
    TurnRight = 0;
     TurnLeft = 0;

  }
   else
   {
    Stop = 1;
    GoForward = 0;
    GoBackward = 0;
    PWMab = 0;
     TurnRight = 0;
     TurnLeft = 0;
   }
  if (Xaxis > 20)
  {
    PWMa = (int) (PWMab * Xaxis/128);
    PWMb = PWMab - PWMa;
     TurnRight = 0;
     TurnLeft = 0;
     if (Xaxis > 100)
     {
      TurnRight = 1;
      PWMb = PWMa;
     }
  }
   else if (Xaxis < -20)
  {
    PWMb = (int) (PWMab * Xaxis/128 *-1);
    PWMa = PWMab - PWMb;
    TurnLeft = 0;
    TurnRight = 0;
     if (Xaxis < -100)
     {
      TurnLeft = 1;
      PWMa = PWMb;
     }
  }
  else
  {
    PWMa = PWMab;
    PWMb = PWMab;
    TurnLeft = 0;
    TurnRight = 0;
  }
  Serial.println("PWMY");
  Serial.println(PWMa);
  Serial.println(PWMb);

}


void HbridgePinout(void)
{
  if (GoForward == 1 and TurnRight == 0 and TurnLeft == 0)
  {
    IN1v = HIGH;
    IN2v = LOW;
    IN3v = HIGH;
    IN4v = LOW;
  }
  else if (GoBackward == 1 and TurnRight == 0 and TurnLeft == 0)
  {
    IN2v = HIGH;
    IN1v = LOW;
    IN4v = HIGH;
    IN3v = LOW;
  }
  else if (TurnRight == 1)
  {
    IN1v = HIGH;
    IN2v = LOW;
    IN3v = LOW;
    IN4v = HIGH;
  }
   else if (TurnLeft == 1)
  {
    IN1v = LOW;
    IN2v = HIGH;
    IN3v = HIGH;
    IN4v = LOW;
  }
  else
  {
    IN1v = LOW;
    IN2v = LOW;
    IN3v = LOW;
    IN4v = LOW;
  }
  digitalWrite(IN1p, IN1v);
  digitalWrite(IN2p, IN2v);
  digitalWrite(IN3p, IN3v);
  digitalWrite(IN4p, IN4v);
  analogWrite(rightMotorPin, PWMa);
  analogWrite(leftMotorPin, PWMb);

   Serial.println("Forward");
   Serial.println(GoForward);
   Serial.println("back");
   Serial.println(GoBackward);

}

int zmierzOdleglosc() {
  long czas, dystans;

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  czas = pulseIn(echoPin, HIGH);
  dystans = czas / 58;

  return dystans;
}

void setup(void)

{
  servos[0].attach(Serwo1p); // Przypisane serwomechanizmów do pinów
  servos[1].attach(Serwo2p);
  servos[2].attach(Serwo3p);
  servos[3].attach(Serwo4p);

  Serial.begin(115200); // Odpalenie transmisji monitoru portu szeregowego
  radio.begin();
  radio.setPALevel(RF24_PA_HIGH); // ustawienie mocy nadajnika
  radio.enableDynamicPayloads(); // uruchomienie dynamicznych payload
  radio.setRetries(5,15); // ustawianie liczby retransmisji

  // Uruchomienie rur komunikacyjnych
  radio.openWritingPipe(pipes[1]);
  radio.openReadingPipe(1,pipes[0]);

  //Start radia
  radio.startListening(); // Rozpoczecie nasłuchu
  Serial.println("Koniec setupu"); // informacja do debugowania

  //Ustawienia portów arduino
  pinMode(leftMotorPin, OUTPUT);
  pinMode(rightMotorPin, OUTPUT);

  //ustawienie dht
   dht.setup(DHT11_PIN);

  // Ustawienia czujnikla odleglosci
  pinMode(trigPin, OUTPUT); //Pin, do którego podłączymy trig jako wyjście
  pinMode(echoPin, INPUT); //a echo, jako wejście


}

void loop(void)
{
  while ( radio.available() ) // Jeżeli są do dane do odebrania
  {
    // Odbierz dane i zobacz czy był to ostatani bit
    uint8_t len = radio.getDynamicPayloadSize();
    // Jeżeli ładunek jest uszkodzony to go porzuć
    if(!len){
      continue;
    }
    radio.read( receive_payload, len );
    // Ustaw zero na końcu 0 dla łatwego wyświetlania wyniku
    receive_payload[len] = 0;


    for(int i = 0; i < len; i++) // Wydrukowanie ładunku
{
  Serial.println(receive_payload[i], DEC );
}
    MotorSpeed(receive_payload[1], receive_payload[2], receive_payload[0]);
    HbridgePinout();
    SerwoControl(receive_payload[3],receive_payload[4], receive_payload[5], receive_payload[6], receive_payload[7], receive_payload[8]);
    //Utworzenie payloadu do wysłania
    int wilgotnosc = dht.getHumidity();
    int temperatura = dht.getTemperature();

    sending_payload[0] = wilgotnosc;
    sending_payload[1] = temperatura;
    sending_payload[2] = 5; // Ustawione tylko do sprawdzenia poprawności komunikacji i konwersji danych

    // Przestań nasłuchiwać, żeby można nadać
    radio.stopListening();

    // Nadawanie wartości
    radio.write(sending_payload, 2);

    Serial.println(F("Sent response."));

    // Wznów nasłuchiwanie dla kolejnych pakietów
    radio.startListening();
  }
}
// vim:cin:ai:sts=2 sw=2 ft=cpp