#define VELIKOST_BEZP_ZN 2   //počet oěřovacích znaků vysílání (dva začáteční byty vysílání)
#define VELIKOST_HODNOTY 2   //velikost odesílané hodnoty
#define CHECKSUM_SIZE 1      //koncový byte, ověření, zda poslaná hodnota je v pořádku
#define VELIKOST_PACKETU (VELIKOST_BEZP_ZN + VELIKOST_HODNOTY + CHECKSUM_SIZE)

#define ADR_ovladac_ESC 5 //adresa přijmu rychlosti, dále pro tuto cestu bude použit 1, pouzti ve funkci writeByte()
#define ADR_ESC_ovladac 251 // adresa odesílání info o baterce atd z ESC do přijímače, dale pro tuto cestu bude použito 2, použití ve funkce readByte()

//LEDky
#define LED_28 8
#define LED_29 9
#define LED_30 10
#define LED_31 11
#define LED_32 12
#define LED_33 13
#define LED_START 4
#define LED_STOP 3
#define T_START_STOP 5
#define T_STAV 7
#define LED_plyn 6


const byte bezpec_znaky_1[VELIKOST_BEZP_ZN] = {3, ADR_ovladac_ESC}; // random číslo jako bezp. znak, pr
const byte bezpec_znaky_2[VELIKOST_BEZP_ZN] = {253, ADR_ESC_ovladac};

//tlačítka
int tlacitko_START_STOP = 0;
int tlacitko_stav_baterie = 0;


//proměnné startu nebo zastavení
int start = 0;
int start_predchozi = 0;
int stopp = 0;

//napětí
byte pocet_LED = 0;

//ostatní pomocné promněnní
int a = 0;
int cas_cekani_prijem = 0;

//filtrace tlačítka
long debounce = 0;
volatile boolean debounce_bool = false;

//plyn, rychlsot, rampa
int plyn = 0;
int mRychlost = 11;
int pRychlost = 0;
int mRychlost_minula = 0;
int rampa = 100; //cas
int cas_1 = 0;



void setup() {

  noInterrupts();

  //pinMode(T_STAV, INPUT);
  pinMode(LED_plyn, OUTPUT);
  pinMode(A0, INPUT);
  pinMode(T_START_STOP, INPUT);
  pinMode(LED_START, OUTPUT);
  pinMode(LED_STOP, OUTPUT);
  pinMode(LED_28, OUTPUT); //PB0
  pinMode(LED_29, OUTPUT); //PB1
  pinMode(LED_30, OUTPUT); //PB2
  pinMode(LED_31, OUTPUT); //PB3
  pinMode(LED_32, OUTPUT); //PB4
  pinMode(LED_33, OUTPUT); //PB5
  Serial.begin(2400);

  //Nastavení timeru 1 pro filtraci tlačítek
  TCCR1A = 0;
  TCCR1B = 0;
  TIMSK1 = 0;
  TCNT1 = 0;

  OCR1A = 50000;
  TCCR1B = (1 << WGM12) | (1 << CS11) | (1 << CS10); //předělička 64
  TIMSK1 = (1 << OCIE1A);
  interrupts(); //globalni povolení přerušení

}

ISR(TIMER1_COMPA_vect)
{
  debounce_bool = false;
}




void writeByte(byte hodnota) //funkce pro vysílání hodnot plynu a tlačítek
{
  byte checksum = ~hodnota;
  Serial.write(1);  // This gets reciever in sync with transmitter
  Serial.write(bezpec_znaky_1, VELIKOST_BEZP_ZN); //Serial.write(buf, len) - buf = an array to send, len = arrays length
  Serial.write(checksum); //CHECKSUM_SIZE
}

byte readByte (boolean wait)
{
  byte pos = 0;
  byte val;
  byte c = 0;

  if ((Serial.available() < VELIKOST_PACKETU) && (wait == false)) //pokud je v bufferu seriové linky více bytu nž velikostu paketu, nepřečte se
  {
    return 0;
  }
  cas_cekani_prijem = millis();

  while (pos < VELIKOST_BEZP_ZN) //VELIKOST_BEZP_ZN určuje velikost pole
  {
    while (Serial.available() == 0) //dokud je buffer prázdný, nepokračuje a čeká na číslo
    {
      if ((millis() - cas_cekani_prijem) > 1000) //pokud trvá čekání na příjem déle než vteřinu, vrátí hodnotu 0 a program pokračuje dále
      {
        return 0;
      }

    }


    c = Serial.read(); //přečte obsah bufferu

    if (c == bezpec_znaky_2[pos]) //odpovídá přečtená informace některému prvku pole?
    {
      if (pos == VELIKOST_BEZP_ZN - 1) //poslední pozice pole, adresa vysílače, teprve poté dojde k načtení posílané hodnoty plynu
      {
        byte checksum;

        while (Serial.available() < 2); //dokud je v bufferu méně než součet velikosti, čekej
        val = Serial.read();
        checksum = Serial.read();

        if ((checksum^val)!=255)
        {
          pos = -1;
        }
      }
      ++pos;
    }
    else if (c == bezpec_znaky_2[0])
    {
      pos = 1;
    }
    else
    {
      pos = 0;
      if (!wait)
      {
        return 0;
      }
    }
  }
  return val;
}


void loop() {

  //since_debounce_stop = TCNT1 - debounce_stop + 65535*ovf;

  //START
  if (tlacitko_START_STOP == 1 && debounce_bool == false && a == 0)
  {
    start = 1;
    stopp = 0;
    a++;
    TCNT1 = 0;
    debounce_bool = true;
    mRychlost = 11;
  }
  //Serial.println(x);


  //STOP
  if (tlacitko_START_STOP == 1 && start == 1 && debounce_bool == false)
  {
    start = 0;
    stopp = 1;
    a = 0;
    TCNT1 = 0;
    debounce_bool = true;
    mRychlost = 11;
  }


  plyn = analogRead(A0); //čtení hodnoty z potenciometru plynu
  pRychlost = map(plyn, 0, 1023, 11, 250); //přemapování na hodnoty 6 - 255.

  //Rampa
  if ((millis() - cas_1) > rampa)
  {
    if (pRychlost > mRychlost)
    {
      mRychlost++;
    }

    if (pRychlost == mRychlost)
    {
      mRychlost = mRychlost_minula;
    }
    cas_1 = millis();

  }

  if (pRychlost < mRychlost)
  {
    mRychlost--;
    //mRychlost = pRychlost;
  }


  if (start_predchozi != start && start == 1) //spuštění motoru
  {
    writeByte(2);
  }
  else if (start_predchozi != start && start == 0) //zastaveni motoru
  {
    writeByte(4);
  }
  else
  {
    writeByte(mRychlost); //rychlost
  }

  if (start == 1) //motor (a interupty) aktivní
  {
    digitalWrite(LED_START, HIGH);
    digitalWrite(LED_STOP, LOW);
  }
  if (start == 0) //motor (a interrupy) neaktivní
  {
    digitalWrite(LED_STOP, HIGH);
    digitalWrite(LED_START, LOW);
  }
  
  analogWrite(6, mRychlost);
  start_predchozi = start;
  tlacitko_START_STOP = digitalRead(T_START_STOP);
  Serial.println(mRychlost);
  mRychlost_minula = mRychlost;

  //napětí baterie, přijem informace z ESC a poté rozsvícení počtu diode podle přijaté hodnoty

  /*tlacitko_stav_baterie = digitalRead(T_STAV);
  if (tlacitko_stav_baterie == 1)
  {
    writeByte(9); //hodnota indikující zmáčknutí tlačítka
    pocet_LED = readByte(true);

    switch (pocet_LED)
    {
      case 1:
        PORTB = B00000001; //baterie na 28 V nebo méně, svítí pouze jedna ledka
      case 2:
        PORTB = B00000011; //28-29V
      case 3:
        PORTB = B00000111; //29-30 V
      case 4:
        PORTB = B00001111; //30-31 V
      case 5:
        PORTB = B00011111; //31-32 V
      case 6:
        PORTB = B00111111; //32-33 V
      default:
        PORTB = B00000000;
    }
  }*/
}