//LED na boardu - červená  = standby, zelena = ready

#define VELIKOST_BEZP_ZN 2
#define VELIKOST_HODNOTY 1 //posílám 1 byte
#define CHECKSUM_SIZE 1 //checksum je ověřovací znak
#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, použiti ve funkce readByte()
#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, pouziti ve funkce writeByte()

#define PWM_A 11
#define PWM_B 10
#define PWM_C 9
#define KOM_A 12
#define KOM_B 6
#define KOM_C 4
#define HALL_A 7
#define HALL_B 2
#define HALL_C 3
#define PROUD A2
#define ENABLE A1
#define AKUMULATOR A4
#define FAULT 8
#define LED_ON_OFF A0
#define LED_STANDBY A5
#define LED_READY A3

const byte bezpec_znaky_1[VELIKOST_BEZP_ZN] = {3, ADR_ovladac_ESC};
const byte bezpec_znaky_2[VELIKOST_BEZP_ZN] = {253, ADR_ESC_ovladac};


//inicializace základních proměnnýh
volatile byte HallHodnota = 0; //binární součet výstupu z hallovek
//volatile byte predchoziHallHodnota = 0;
volatile int mRychlost = 0; //rychlost motoru

//měření otáček
float otacky_cas_1 = 0;
float otacky_cas_2 = 0;
float cas_el_otacky = 0;
float RPM_okamzite = 0;
float RPM_avg = 0;
boolean otacky = false; 
int i = 0;
float otacky_pom = 0;

//měření proudu
int proud_pom = 0;
int proud_okamzity = 0;
int proud_soucet = 0;
int proud = 0;
int j = 0;

//měření napětí
int napeti_ak_analog = 0; //hodnota na vstupu
int napeti_ak = 0; //přepočítaná hodnota na volty
byte pocet_LED = 0; //počet rozsvícených LED na ovladači

//ostatni
int tlacitkoReset = 0; //mžikové tlačítko
boolean start = false;
double cas_rychlost = 0;
double cas_rychlost_minula = 0;
boolean staci = false;
int mRychlost_minula = 0;
int stopp = 0;
int start_predchozi = 0;
int pole[] = {4, 5, 1, 3, 2, 6};

//cas behu jednoho cyklu
double delta_cas_programu = 0;
double cas_programu_1 = 0;
double cas_programu_2 = 0;
unsigned int cas_cekani_prijem = 0;

unsigned int pom = 0;

volatile boolean stall = false;

void setup() {

  noInterrupts(); //globalni zakaz preruseni
  pinMode(PWM_A, OUTPUT);    // PWM_A
  pinMode(PWM_B, OUTPUT);    // PWM_B
  pinMode(PWM_C, OUTPUT);     // PWM_C
  pinMode(KOM_A, OUTPUT);    // KOM_A
  pinMode(KOM_B, OUTPUT);     // KOM_B
  pinMode(KOM_C, OUTPUT);     // KOM_C
  pinMode(HALL_A, INPUT);      //hall fáze A
  pinMode(HALL_B, INPUT);      //hall fáze B
  pinMode(HALL_C, INPUT);      //hall fáze C
  pinMode(PROUD, INPUT);     //měření proudu
  pinMode(ENABLE, OUTPUT); //driver enable
  pinMode(AKUMULATOR, INPUT); //napětí baterie
  pinMode(FAULT, INPUT); //signal fault

  //LEDky
  pinMode(LED_ON_OFF, OUTPUT); //LED ON/OFF
  pinMode(LED_STANDBY, OUTPUT); //LED Standby
  pinMode(LED_READY, OUTPUT); //LED Ready

  Serial1.begin(2400); //Serial1 je seriová komunikace přes TX a RX piny; přijmací modul může fungovat až do 2400, nicméně se zmenšuje dosah
  Serial.begin(2400); //seriová komunikace přes USB

  //incializace PWM - Compare Output Mode, Phase Correct and Phase and Frequency Correct PWM
  TCCR1A = 0; //Timer/Counter1 Control Register A - vynulování
  TCCR1B = 0; //Timer/Counter1 Control Register B - vynulování

  //PWM, 8-bit, clear OC3x on upcounting caompare, set OC3x on downcouting compare
  TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << COM1C1) | (1 << WGM10); //nastavení COM1x1 na jedničku nastaví "Clear OCnA/OCnB/OCnC on compare match when upcounting.
  //Set OC1A/OC1B/OC1C on compare match when down-counting (OC1A => PWM_A, OC1B => PWM_B, OC1C => PWm_C
  //(1<<WGM13) | (1<<WGM10) nastavuje Phase and Frequency Correct PWM, 8-bit režim
  TCCR1B =  (1 << CS10); //nastavení předěličky (1<<CS11) je předělička 8
  //(1<<CS10) je předělička 1 => frekvence 32 500 Hz

  OCR1A = 0; //Output Compare Register, timer je porovnáván právě s touto hodnotou
  OCR1B = 0;
  OCR1C = 0;
  PORTD  =  (1 << PD4) | (1 << PD7) | (1 << PD6);


  TCCR3A = 0; //Timer/Counter3 Control Register A – vynulování, nic jineho, nechci ani PWM, ani capture pins

  //Normal port operation, OC3A disconnected (pin 5)
  TCCR3B = 0; //Timer/Counter3 Control Register B – vynulování

  TCCR3B = (1 << CS31) | (1 << CS30); //předělička 64
  TCNT3 = 0; //start, max hodnota 65535
  interrupts(); //globalni povolení přerušení

  //Externí přerušení

  attachInterrupt(digitalPinToInterrupt(7), KOMUTACE, CHANGE); 
  attachInterrupt(digitalPinToInterrupt(2), KOMUTACE, CHANGE);
  attachInterrupt(digitalPinToInterrupt(3), KOMUTACE, CHANGE);

  volnobeh(); //zablokuje všechny tranzistory a zakáže external interupty
}


//Vysílač
void writeByte(byte hodnota) //funkce pro vysílání hodnot plynu a tlačítek
{
  byte checksum = ~hodnota; // operace /256 je bit shift o 8 doprava // děleno dvěma je posun o jeden but doprava, 256 je tato opreace 8 krát zopakovaná
  Serial1.write(1);  // This gets reciever in sync with transmitter
  Serial1.write(bezpec_znaky_2, VELIKOST_BEZP_ZN); //Serial.write(buf, len) - buf = an array to send, len = arrays length
  Serial1.write(hodnota);
  Serial1.write(checksum); //CHECKSUM_SIZE
}


//přijímač
byte readByte (boolean wait)
{
  unsigned int cas_prijem = 0;
  long cas_prijem_s = 0;
  byte pos = 0;
  byte val;
  byte c = 0;
  int ovflow = 0;


  cas_cekani_prijem = TCNT3;

  while (pos < VELIKOST_BEZP_ZN) //VELIKOST_BEZP_ZN určuje velikost pole
  {
    cas_prijem = TCNT3 - cas_cekani_prijem;
    if (cas_prijem < 0)
    {
      cas_prijem += 65535;
    }
    
    if (cas_prijem > 60000) //při čekání dele než  1 s vrátí hodnotu 11 a program pokračuje dale
      return 11;

    while (Serial1.available() == 0);

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

    if (c == bezpec_znaky_1[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 (Serial1.available() < 2); //dokud je v bufferu méně než součet velikosti, čekej
        val = Serial1.read();
        checksum = Serial1.read();

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




//Komutace

void KOMUTACE ()
{

  //if (stall == false)
  //{
    if ((PINE & 0b01000000) == 0)
    {
      HallHodnota = (PIND & 0b00000011); //okamžité načtení, rychlejší než digitalread
    } else {
      HallHodnota = (PIND & 0b00000011) + 4; //okamžité načtení, rychlejší než digitalread
    }
 // }
  /*if(proud > 60)
    HallHodnota = 7; //default, blokace všech tranzistorů
  */
  switch (HallHodnota)
  {
    case 6:
      //Fáze A = PWM;Fáze B = 0;Fáze C = oo;LIN_B -> pin TXLED (PD7) - 0;LIN_C -> pin 4 (PD4) - 1;LIN_A =  pin 12 (PD6) - 0
      PORTD  =  (1 << PD4) | (0 << PD7) | (0 << PD6); //LIB_C, LIN_B, LIN_A
      OCR1A = 0; // pin 9 (PWM_C)
      OCR1B = 0; // pin 10 (PWM_B)
      OCR1C = mRychlost; // pin 11 (PWM_A)

      otacky_cas_1 = otacky_cas_2;
      otacky_cas_2 = TCNT3;
      otacky = true;

      break;

    case 2:
      //Fáze A = PWM;Fáze B = oo;Fáze C = 0;LIN_B -> pin TXLED (PD7) - 1;LIN_C -> pin 4 (PD4) - 0;LIN_A =  pin 12 (PD6) - 0
      PORTD  =  (0 << PD4) | (1 << PD7) | (0 << PD6); //LIB_C, LIN_B, LIN_A)
      OCR1A = 0; //pin 9 (PWM_C)
      OCR1B = 0; // pin 10 (PWM_B)
      OCR1C = mRychlost; //pin 11 (PWM_A)
      break;

    case 3:
      //Fáze A = oo;Fáze B = PWM;Fáze C = 0;LIN_B -> pin TXLED (PD7) - 0;LIN_C -> pin 4 (PD4) - 0;LIN_A =  pin 12 (PD6) - 1
      PORTD  =  (0 << PD4) | (0 << PD7) | (1 << PD6); //LIB_C, LIN_B, LIN_A)
      OCR1C = 0; // pin 11 (PWM_A)
      OCR1A = 0; // pin 9 (PWM_C)
      OCR1B = mRychlost; //pin 10 (PWM_B)
      break;

    case 1:
      //Fáze A = 0; Fáze B = PWM; Fáze C = oo;LIN_B -> pin TXLED (PD7) - 0;LIN_C -> pin 4 (PD4) - 1;LIN_A =  pin 12 (PD6) - 0
      PORTD  =  (1 << PD4) | (0 << PD7) | (0 << PD6); //LIB_C, LIN_B, LIN_A)
      OCR1A = 0; // pin 9 (PWM_C)
      OCR1C = 0; //pin 11 (PWM_A)
      OCR1B = mRychlost; // pin 10 (PWM_B)
      break;

    case 5:
      //Fáze A = 0; Fáze B = oo; Fáze C = PWM;LIN_B -> pin TXLED (PD7) - 1;LIN_C -> pin 4 (PD4) - 0;LIN_A =  pin 12 (PD6) - 0
      PORTD  =  (0 << PD4) | (1 << PD7) | (0 << PD6); //LIB_C, LIN_B, LIN_A)
      OCR1B = 0; //pin 10 (PWM_B)
      OCR1C = 0; //pin 11 (PWM_A)
      OCR1A = mRychlost; // pin 9 (PWM_C)
      break;

    case 4:
      //Fáze A = oo; Fáze B =0;Fíze C = PWM;LIN_B -> pin TXLED (PD7) - 0; LIN_C -> pin 4 (PD4) - 0; LIN_A =  pin 12 (PD6) - 1
      PORTD  =  (0 << PD4) | (0 << PD7) | (1 << PD6); //LIB_C, LIN_B, LIN_A)
      OCR1B = 0; // pin 10 (PWM_B)
      OCR1C = 0; // pin 11 (PWM_C)
      OCR1A = mRychlost; // pin 9 (PWM_C)
      break;

    default:

      PORTD  =  (1 << PD4) | (1 << PD7) | (1 << PD6); //LIB_C, LIN_B, LIN_A)
      OCR1A = 0; // pin 9 (PWM_C)
      OCR1B = 0; // pin 10 (PWM_B)
      OCR1C = 0; // pin 11 (PWM_A)
      break;
  }
}

void volnobeh() //zablokuje všechny mosfety, motor doběhne
{
  EIMSK = (0 << INT0) | (0 << INT1) | (0 << INT6); //vypnutí external interruptu pro piny hallovych senzoru
  EIFR = (1 << INTF0) | (1 << INTF1) | (1 << INTF6); //odstranění flags způsobenych při doběhu motoru

  PORTD  =  (1 << PD4) | (1 << PD7) | (1 << PD6); //LIB_C, LIN_B, LIN_A)
  OCR1A = 0; // pin 9 (PWM_C)
  OCR1B = 0; // pin 10 (PWM_B)
  OCR1C = 0; // pin 11 (PWM_A)
}

void povoleni_exint () // povlení externích přerušení pro komutaci
{
  //EICRA = (1 << ISC00) | (0 << ISC01) | (1 << ISC10) | (0 << ISC11);
  //EICRB = (1 << ISC60) | (0 << ISC61);

  EIMSK = (1 << INT0) | (1 << INT1) | (1 << INT6);
}

void START () //spuštění motoru
{
  povoleni_exint ();
}

void netoci()
{
  Serial.println("netoci");
  stall = true;
  int prvek_cislo = 0;
  if ((PINE & 0b01000000) == 0)
  {
    HallHodnota = (PIND & 0b00000011); //okamžité načtení hodnoty
  } else {
    HallHodnota = (PIND & 0b00000011) + 4; //okamžité načtení hodnoty
  }
  for (int h = 0; h < 6; h++)
  {
    if (pole[h] == HallHodnota)
    {
      prvek_cislo = h;
      break;
    }
  }
  KOMUTACE();
  for (int m = 0; m < 3; m++)
  {
    prvek_cislo++;
    if (prvek_cislo > 5)
      prvek_cislo = 0;

    HallHodnota = pole[prvek_cislo];
    KOMUTACE();
    delay(5);
  }
  stall = false;


}
//Hlavní smyčka programu
void loop() {
  //Serial.println("loop");

  pom = TCNT3 - otacky_cas_2;
  if (pom < 0)
    pom += 65535;

  // Serial.println(pom);
  if ((pom > 60000) && start == 1)
  {
    netoci();
    //Serial.println("netoci");
  }

  int indikace_stavu = 0;

  if (start_predchozi != start) //pouze po zmáčknutí zmáčknutí talčíka pro start se obslouží podmínka
  {
    if (start == 1)
    {
      START();
    }

    //STOP
    if (stopp == 1)
    {
      volnobeh();
    }
  }

  //měření otáček
  if (otacky)
  {
    cas_el_otacky = (otacky_cas_2 - otacky_cas_1) * 0.004; //timer s předěličkou 64, jeden tik trvá 0.000004 s, s tak malým číslem Arduino neumí pracovat, proto přenásobeno *1000
    if (cas_el_otacky < 0) //přetečení
    {
      cas_el_otacky = cas_el_otacky + 65535 * 0.004; // timer je 16 bitový, přeteče po 2^16 - 1
    }
    RPM_okamzite = 10 / (cas_el_otacky); //6 pol.párů, vzorec rpm = (60*f)/p => 60/(T*p) =* 10/T
    otacky_pom = RPM_okamzite + otacky_pom; //aritemtický průměr z 20 hodnot otáček
    otacky = false;
    i++;
    if (i == 20)
    {
      RPM_avg = 1000 * (otacky_pom / i); //doba tiku byla pro počítání 1000x zvětšena, proto musím ted přenásobit
      i = 0;
      //Serial.print("RPM: ");
      //Serial.println((float)RPM_avg);
      otacky_pom = 0;
    }

  }
  proud_pom = analogRead(A2);
  proud_okamzity = map(proud_pom, 0, 1023, 0, 83); //shunt rezistor 0,3 mOhm -> zesilovač x200 -> pro I = 60 A je U = 3,6 V. 1023 je 5 V a tomu odpovídá proud I = 83 A
  proud_soucet = proud_okamzity + proud_soucet;
  j++;
  if (j == 20)
  {
    proud = proud_soucet / j;
    proud_soucet = 0;
    j = 0;
  }

  //Rychlost motoru, čtení z ovladače
  mRychlost_minula = mRychlost; //pomocná pro případ hodnota mRychlost mimo interval <5;255>
  start_predchozi = start; //pomocná zajištující čtení if() startu jen při změně hodnoty start
  mRychlost = readByte(true); //čtení bezdrátové komunikace
  if (mRychlost == 254) //nepřichází signál od ovladače
  {
    volnobeh();
  }
  //Serial.println(mRychlost);

  if ((mRychlost < 6 || mRychlost > 250) && mRychlost != 2 && mRychlost != 4 && mRychlost != 255) //hodnoty tlačítek z ovladače jsou mapovány na 0 - 5
  {
    mRychlost = mRychlost_minula;
  }
  else if (mRychlost == 2) //spuštení motoru
  {
    start = 1;
    stopp = 0;
    Serial.println("start");
  }
  else if (mRychlost == 4) //zasteveni motoru
  {
    start = 0;
    stopp = 1;
    Serial.println("stop");
  }
  /*else if (mRychlost == 9)
    {
    indikace_stavu = 1; //info pro poslání udaju o baterce
    }*/



  //Čtení stavu akumulátoru a vysílání ovladači
  /*napeti_ak_analog = analogRead(AKUMULATOR);
    napeti_ak = map(napeti_ak_analog,0,1023,0,350) // napětí baterky získám podělením 10*/
  //475 je při maxilmánlní napětí, tj 33.6 V, minimálníé, po kteérm už vypnu je 28 V (3.5V na  článek), 5V bybylo při 35.3 V

  //podle napětí baterie uložím hodnotu do proměnné počet_LED, ta rozhoduje kolik LED diod bude svítit, přičemž 1 LED = 1 volt, a poté toto číslo pošlu ovladači, který diody rozsvítí
  /*
    if(indikace_stavu == 1)
    {
    if(napeti_ak >= 320)
      pocet_LED = 6;
    if(napeti_ak >= 310 && napeti_ak < 320)
      pocet_LED = 5;
    if(napeti_ak >= 300 && napeti_ak < 310)
      pocet_LED = 4;
    if(napeti_ak >= 290 && napeti_ak < 300)
      pocet_LED = 3;
    if(napeti_ak >= 280 && napeti_ak < 290)
      pocet_LED = 2;
    if(napeti_ak <280)
      pocet_LED = 1;

    writeByte(pocet_LED); //odeslíání informace ovladači

    }
  */

  /*
    cas_programu_1 = TCNT3;
    delta_cas_programu = (cas_programu_1 - cas_programu_2)*0.4;
    if (delta_cas_programu < 0) //přetečení
      {
        delta_cas_programu = delta_cas_programu + 65535*0.4; // timer je 16 bitový, přeteče po 2^16 - 1
      }
    Serial.println(delta_cas_programu);


    cas_programu_2 = cas_programu_1;
  */
}