/*
Ovladac motoru
*/
//#pragma GCC optimize ("-O3")

#include <aes.h>
#include <RF22Datagram.h>
#include <RF22.h>
#include <SPI.h>
#include <avr/interrupt.h>
#include <EEPROM.h>
#include "gatec.h"
#include "eepromrw.h"


buffert             b;                          // union bufferu
ramBaset            r;                          // control structure

uint8_t             motorCmd;                   // command for motor
uint16_t            motorOffTime = 0;           // in miliseconds
uint16_t            motorTimeMax = 60000;       // in miliseconds
uint16_t            currentAverageMax = CurrentMax*CurrentValues;          
uint16_t  volatile  currentAverage = 0;         // moving averenge
uint16_t  volatile  motorTime = 0;              // time from motor start
uint8_t   volatile  motorRunning = 0;           // motor is running flag
bool      volatile  motorDirection = DirectionClose;  // actual motor direction
uint8_t   volatile  current[CurrentValues];     // last curent values table, resolutiom 0.019531A
uint8_t   volatile  currentIndex = 0;           // index for next value to be written
uint8_t   volatile  currentStoreIndex;          // index to store array

uint8_t   volatile  gateRunning = 0;            // gate is running flag

uint8_t   volatile  registerTimeout;            //  waiting for register confirmation from master
uint16_t  volatile  miliseconds = 0;
uint32_t  volatile  seconds = 0;

uint8_t             key[16];              // encryption key RAM copy
uint8_t             device;               // device number found for serial number
uint8_t             from;                 // message received or transmit from  
uint8_t             to;                   // message received or transmit to
uint8_t             len;                  // message length to transmit or received
uint8_t             id;                   // id of message
uint8_t             flags;                // no function recently

const uint8_t      *pchar = (uint8_t *) (&b.buf[0]);  // pointer to transmit/receive buffer
const baset        *pee = 0;              // pointer to eeprom structure

uint8_t volatile    button;               // button value
uint8_t             function;             // function value
uint8_t             mode = 0;             // control mode 0 = basic mode
uint16_t volatile   modeTimeout = 0;      // control mode time in seconds

uint16_t            ledOnTime;            // led on time in ms  0 = LedPin OFF 
uint16_t            ledPeriod;            // led period  in ms  0 = only ON pulse
uint8_t  volatile   ledPulseCount = 0;    // number of pulses   255 = infininite (to change)
uint16_t volatile   ledCounter = 0 ;      // actual led counter
bool     volatile   ledOn = 0;            // led is ON   


RF22Datagram        rf22(0);


//********************************* PACKET ********************************************

// search serialn in registered devices 
// if found and from == lowest byte of serialn return index to device
uint8_t  searchSerialn(uint32_t serialn, uint8_t from) 
{
  uint32_t     eeserialn;
  uint8_t      i;  
  uint8_t      devices;  
  
  eepromr( (uint32_t)(&(pee->devices)), devices, sizeof(devices) ); 
      dprint("Devices=");
      dprint(devices);
  
  for (i = 1; i <= devices; i++)
  {
    eepromr( (uint32_t)(&(pee->device[i].serialn)), eeserialn, sizeof(eeserialn) );
    if ( (serialn == eeserialn) && (serialn & 0xff) == from  ) {
      dprint(" REMOTE SERIAL ");
      dprintHex(b.remote.serialn, HEX);
      dprint(" FOUND AS DEVICE =");
      dprintln(i);
      device = i;
      return i;
    }  
  }    
  
  dprint(" REMOTE SERIAL NOT FOUND ");
  dprintHex(b.remote.serialn, HEX);
  dprintln();
  return 0;  
}


bool checkcounter(uint8_t device) {
  
  uint32_t  counter[Counters];
  uint32_t  diff  = 0xFFFFFFF;
  uint16_t  window;
  int8_t    index = -1;
  bool      match = false;
  
  eepromr((uint32_t)(&(pee->counterWindow)), window, sizeof(window) );    
  eepromr((uint32_t)(&(pee->device[device].counter[0])),counter[0], sizeof(counter) );
  
  
  // find the closest lower counter value
  for (uint8_t i=0; i<Counters; i++) {          
    if ( b.remote.counter - counter[i] < diff ) {
      diff = b.remote.counter - counter[i];
      index = i;
    }
  }
  
  // if valid counter value
  if ( index >= 0 && diff && diff <= window )  {
    index++;
    if ( index >= Counters )    
      index = 0;
    eepromw((uint32_t)(&(pee->device[device].counter[index])), b.remote.counter, sizeof(b.remote.counter) );
    match = true;
  }  

   
  dprint("Counter window match=");
  dprint(match);
  dprint(" remote.counter =");
  dprintHex(b.remote.counter, HEX);
  dprint(" eecounter diff=");
  dprintHex(diff, HEX);
  dprintln();
  return match;

}  

//********************************* RECEIVE ********************************************


// receive remote packet and control gate
uint8_t receivePacket() {
  uint8_t   function;
  device = getPacketDevice();

  if ( (b.remote.serialn & 0xff) != from)
    return 0;
 
  if ( device ) {                         // found in list of devices    
    if ( !checkcounter(device) )          // if incorrect counter window - error
      return 0;
    
    switch (id) {                         // command type
      
      case IdRemoteS:                     // remote key command
        if (mode == ModeLearn ) {                   // in learn mode add new function
           addRemoteFunction(device);
           stopLearnMode(1);   
        }   
        else  
        //  if (b.remote.command == CmdRemoteKey )
             if ( function = getRemoteFunction(device) ) {             
               motorCmd = function; // set motor command
               ledPulse(1,500);
             }   
        break;
        
      case IdGateS:
           if (mode == ModeLearn) {
             uint8_t master=0;
             eepromw( (uint32_t)(&(pee->slaveWingIdx)), device, sizeof(device) );    // we are master now
             eepromw( (uint32_t)(&(pee->masterWingIdx)), master, sizeof(master) );   // and therefore no slave  
             sendCommand(from, CmdGateMaster);                                       // command for slave to register master
             rf22.waitPacketSent();
             dprintln("Registered as master, device gate slave is present");
             stopLearnMode(1);
          }  
          break;

      default:
             
        break;
    }
  }   
  else {
     if (mode == ModeLearn) {
        if ( id == IdRemoteS || b.remote.command == CmdRegister )
          addNewDevice();
          if (id == IdRemoteS) {         // registering function
             addRemoteFunction(device);
          }
        if (id == IdGateS ) {            // registering of slave gate
          uint8_t master=0;
          eepromw( (uint32_t)(&(pee->slaveWingIdx)), device, sizeof(device) );    // we are master now
          eepromw( (uint32_t)(&(pee->masterWingIdx)), master, sizeof(master) );   // and therefore no slave  
          sendCommand(from, CmdGateMaster);                                       // command for slave to register master
          rf22.waitPacketSent();
          dprintln("Registered as master, device gate slave is present");
        }  
        stopLearnMode(1);
     }

     if ( registerTimeout && id == IdGateS) {
        addNewDevice();
        uint8_t slave=0;
        eepromw( (uint32_t)(&(pee->slaveWingIdx)), slave, sizeof(slave) );    // we are slave now
        eepromw( (uint32_t)(&(pee->masterWingIdx)), from, sizeof(from) );     // master has been registered now
        dprintln("Registered as slave");          
     }
  } 
}                                           



// returns device index 1 to n or error = 0
uint8_t getPacketDevice() {
 
  uint8_t   i,j;  
  uint8_t   remoteButton;    

//  D(for (i= 0; i < 16; i++) { eepromr (i,j,1); dprintHex(j,HEX); dprint(" "); }  ; dprintln("");)

  eepromr((uint32_t)(&(pee->key[0])), key[0], sizeof(key)); 

//  D(for (i= 0; i < 16; i++) { dprintHex(key[i],HEX); dprint(" "); }  ; dprintln("");)
//  D(for (i= 0; i < 16; i++) { dprintHex(pchar[i],HEX); dprint(" "); }  ; dprintln("");)
  
  aes(&b.buf[0], key, Decrypt);  
  
  dprintln("");
  dprint("from=");
  dprintHex(from, HEX);
  dprint(" to=");
  dprintHex(to,HEX);
  dprint(" Id=");
  dprintHex(id, HEX);
  dprint(" len=");
  dprintHex(len, HEX);
  dprint(" Button=");
  dprintHex(b.remote.button, HEX);
  D( dprint(" Packet="); for (i= 0; i < 16; i++) { dprintHex(pchar[i],HEX); dprint(" "); }  dprintln(); )
  
  device = searchSerialn(b.remote.serialn, from);

  return device;
}
  

uint8_t  getRemoteFunction(uint8_t device) {

  uint8_t function;
  
  if (id == IdRemoteS)  {
    b.remote.button &= 0xF;
    if ( (b.remote.button == 0) || (b.remote.button == 15) ) {
      dprintln("No valid Key");
      return 0;   // no valid button
    }
            
    // store remote parameters 
    r.device[device].battery     = b.remote.battery;
    r.device[device].temperature = b.remote.temperature;    

    // read function for button
    eepromr( (uint32_t)(&(pee->device[device].function[b.remote.button-1])), function, sizeof(function) );
    if (function < CmdGateOposite || function > CmdGateStop )
      function = 0;

    dprint("Registered function=");
    dprintHex(function, HEX);
    dprintln();

    return function;
  }
}  


// add new device
bool addNewDevice() {
    uint8_t zero = 0;
    
    // check available space
    eepromr( (uint32_t)(&(pee->devices)), device, sizeof(device) );
    
    device++;         
    if ( (uint16_t)&(pee->device[device].function[14] ) > 1023 )
      return false;
      
    // add new device
    eepromw( (uint32_t)(&(pee->devices)), device, sizeof(device) );   // store new device number

    // write new device to eeprom and RAM structures
    eepromw( (uint32_t)(&(pee->device[device].serialn)), b.remote.serialn, sizeof(b.remote.serialn) );
    b.remote.counter-= Counters + 1;
    for (uint8_t i = 0 ; i< Counters; i++, b.remote.counter++) 
      eepromw( (uint32_t)(&(pee->device[device].counter[i])), b.remote.counter, sizeof(b.remote.counter) );        
    eepromw( (uint32_t)(&(pee->device[device].type)),    id, sizeof(id) );
    for (uint8_t i = 0 ; i< sizeof(pee->device[device].function) ; i++) 
      eepromw( (uint32_t)(&(pee->device[device].function[i])), zero  , sizeof(zero) );        
    
    r.device[device].temperature = b.remote.temperature;
    r.device[device].battery     = b.remote.battery;         
    dprint ("Add new: device="); dprintln(device); 
    
    return true;
}


// register function for key
bool  addRemoteFunction(uint8_t device) {
  if (device) {                   // registered yet only function add
    if ( b.remote.button && (id==IdRemoteS) ) {        
      eepromw( (uint32_t)(&(pee->device[device].function[b.remote.button-1])), function, sizeof(function) );
      dprint ("Add Remote function: device="); dprint(device); dprint(" key="); dprint(b.remote.button);  dprint(" function="); dprintln(function); 
      uint8_t i, j;
      D(for (i= 0; i < 32; i++) { eepromr ((uint32_t)(&pee->device[device].function[i-21]),j,1); dprintHex(j,HEX); dprint(" "); }  ; dprintln("");)
//      D(for (i= 0; i < 32; i++) { eepromr ((uint32_t)(&pee->device[device].function[i-21]),j,1); dprintHex((uint32_t)(&pee->device[device].function[i-21]),HEX); dprint(" "); }  ; dprintln("");)
      return true;
    }
  }   
  return false;
}


// remove all registered devices 
// only write 0 to devices
void  removeAllDevices(){
  uint8_t  devices=0;                                                 
  eepromw( (uint32_t)(&(pee->devices)), devices, sizeof(devices) );   // store new device number
  dprintln ("All Devices Removed");

  
}


// checks eeprom counters
// write  next TX counter value to eeprom
// return next TX counter value 
uint32_t getCounterTx() {
  
  uint32_t  counter[Counters];
  uint32_t  getCounter;
  uint32_t  maxCounter;
  uint32_t  minCounter;
  uint8_t   smooth=0;
  uint8_t   smoothMax = 0;
  uint8_t   min = 0;
  uint8_t   max = 0;  
  uint8_t   i;  
  uint8_t   il;  
  uint8_t   ih;    
  uint16_t  window;
  
  eepromr( (uint32_t)(&pee->counterWindow), window, sizeof(window) ); 
  eepromr( (uint32_t)(&pee->device[0].counter[0]),  counter[0], sizeof(counter) ); 

  maxCounter = counter[0];
  minCounter = counter[0];  
  for (uint8_t i=1; i<Counters; i++) {          // find min and max element
    if ( minCounter > counter[i] ) {
      minCounter = counter[i];
      min = i;
    }  
    if ( maxCounter < counter[i] ) {
      maxCounter = counter[i];
      max = i;    
    }  
  }
 /* 
  dprint("min= ");
  dprint(min);
  dprint(" max= ");
  dprint(max);
  dprint(" CounterMin= ");
  dprint(counter[min]);
  dprint(" CounterMax= ");
  dprint(counter[max]);
  dprintln();
*/
  
  // if whole region is not smooth  
  // find largest smooth region where next value is 
  if ( counter[max]-counter[min] > Counters-1 ) {
    // loop from min to max 
    for ( i=0, il=min, ih=min+1; i<Counters; i++, il++, ih++ ) {     // find min and max element
      if (il >= Counters)  il = 0;            // index low in range of round buffer
      if (ih >= Counters)  ih = 0;            // index high in range of round buffer      
      if ( counter[ih] - counter[il] > 1 ) {
        smooth++;
        if (smooth > smoothMax) {               // biggest smooth region found
          smoothMax = smooth;
          max = ih;
        }  
 /*      dprint("il= ");
       dprint(il);
       dprint(" SmoothMax= ");
       dprint(smoothMax);
       dprint(" Smooth= ");
       dprint(smooth);
       dprint(" Max= ");
       dprint(max);
       dprintln();  */
      }
      else 
        smooth = 0;
    }    
  }
  
  getCounter = counter[max] + 1;
/*
  dprint(" max= ");
  dprint(max);
  dprint(" CounterMax= ");
  dprint(counter[max]);
  dprint(" GetCounter= ");
  dprint(getCounter);
  dprintln();
*/
  if ( ++max >= Counters )
    max = 0;
  eepromw((uint32_t)(&pee->device[0].counter[max]),  getCounter, sizeof(getCounter)); // store new counter value to eeprom
  return getCounter;
}



// send data packet with prepared payload
void sendPacket (uint8_t id, uint8_t to, uint8_t len)
{
  int i;
  
  // set header
  eepromr( (uint32_t)(&pee->device[0].serialn), b.remote.serialn, sizeof(b.remote.serialn) );
  from = b.remote.serialn & 0xff;
  
  b.remote.counter = getCounterTx();

  dprintln("");
  dprint("from=");
  dprintHex(from, HEX);
  dprint(" to=");
  dprintHex(to,HEX);
  dprint(" Id=");
  dprintHex(id, HEX);
  dprint(" len=");
  dprintHex(len, HEX);
  dprint(" Command=");
  dprintHex(b.remote.command, HEX);
  D( dprint(" Packet="); for (i= 0; i < 16; i++) { dprintHex(pchar[i],HEX); dprint(" "); }  dprintln(); )

  // encrypt and send
  eepromr((uint32_t)(&(pee->key[0])), key[0], sizeof(key)); 
  aes(&b.buf[0],key,Encrypt);  
  rf22.setHeaderId(id);
  i = rf22.sendto(b.buf, len, to);  
  rf22.waitPacketSent();
  dprint("Result: "); dprintln(i);
  dprint("Packet Sent: "); D(for (i= 0; i < 16; i++) { dprintHex(pchar[i],HEX); dprint(" "); }  ; dprintln("");)
}


// prapare data and send command
void  sendCommand(uint8_t to, uint8_t command, uint16_t par1 , uint16_t par2 ) {
    uint8_t type;
    memset((void*)b.buf, 0, sizeof(b));    // clear buffer
     
    b.gateCmd.command = command;           // set command and parameters
    b.gateCmd.par[0]  = par1;
    b.gateCmd.par[1]  = par2;

    eepromr( (uint32_t)(&pee->device[0].type), type, sizeof(type) );
    sendPacket(type, to, 16);           // send it
    dprint("Send Command to=");  dprint(to);    
    dprint(" command=");  dprint(command);        
    dprint(" type=");  dprint(type);     
    dprintln();
}

void gateCurrentStore(uint8_t sample) {
  if ( currentStoreIndex< CurrentStoreValues )
    r.gateCurrent[currentStoreIndex++] = sample;
}

//********************************* INTERRUPT *******************************************

void intInit() {
  cli();
  TCCR1A = 0;              // setup timer 1 for interrupt every 1 ms
  TCCR1B = 0xA;            // imput is 1 MHz and counts microseconds
  TCCR1C = 0;              // after 1000 us generates an interrupt
  TIMSK1 = 2;
  OCR1AH = 3;
  OCR1AL = 0xE7;
  OCR1BH = 3;
  OCR1BL = 0xE7;
  
  sei();
}


// interrupt routine
ISR(TIMER1_COMPA_vect) {
  
//  digitalWrite(LimitPin, HIGH);
  
  // read buton ------------------------------------------------------
  
  static uint16_t    buttonTime;
/*
  if (digitalRead(ButtonPin))
     digitalWrite(LedPin, HIGH);
  else
      digitalWrite(LedPin, LOW);
*/        
   
  if ( !digitalRead(ButtonPin) ) {   // when button is pushed  
     if (  (buttonTime <= ButtonExtraTime) && !button ) 
       buttonTime++;
       
     if ( buttonTime == ButtonLongTime )
       digitalWrite(LedPin, 0); // led On
        
     if ( buttonTime == ButtonExtraTime )
       digitalWrite(LedPin, 1); // led Off
  }   
  else  {                                                              // when button is releassed short or long button mode  
     if ( !button ) {
       if ( buttonTime >= ButtonShortTime ) button = ButtonShort;       // cca 50 ms
       if ( buttonTime >= ButtonLongTime  ) button = ButtonLong;        // cca 1.5 s
       if ( buttonTime >= ButtonExtraTime ) button = ButtonExtra;       // cca 8 s     
       if ( button )     
         digitalWrite(LedPin, 1);    // turn off LED
     }
     buttonTime = 0;
  }
    
  
  // LedPin control------------------------------------------------------
  
  uint8_t i=0;
  
  if ( (ledCounter == 0) && (ledOnTime) && (ledPulseCount) ) {
    ledOn = true;                  // led ON
    digitalWrite(LedPin, 0);
    digitalWrite(LimitPin, HIGH);
  }  
  
  if ( (ledCounter == ledOnTime) && (ledOn) ) {
    ledOn = false;                  // led OFF
    digitalWrite(LedPin, 1);
    digitalWrite(LimitPin, LOW);
  }  
  
  if (ledPulseCount)
    if (ledCounter++ > ledPeriod) {
      ledCounter = 0;
      if ( ledPulseCount < 255 )
        ledPulseCount--;
    }
  
  // timers ---------------------------------------------------------
  
  if ( registerTimeout  )        // wait for gate registration from master
    registerTimeout--;

 if ( modeTimeout  )             // learn mode timeout
    modeTimeout--;
 
  if ( motorOffTime  )           // motor off time timeout
    motorOffTime--;

 
 if (miliseconds == 999) {       // absolute time from start
       miliseconds = 0;
       seconds++;      
       
  } else
    miliseconds++;  
  
  
  // motor control  ---------------------------------------------------
  // 1. filt  - compute average of CurrentFilterSamples samples and write result to current[] fifo
  // 2. sum   - calculate sum of whole current[] fifo array 
  // 3. store - store values to gateCurrent array for later inspection
  
  
  uint8_t  static  currentFilterCounter;    // counter to CurrentFilterSamples 
  uint32_t static  currentFilterSum;        // average of CurrentFilterSamples 
  uint8_t  static  currentStorePhase;       // Phase 0: xx samples store each currentFilterSum
  uint16_t static  currentStoreCounter;     // Phase 1: store one average after x samples until motor stops
  uint16_t static  currentStoreValue;       // average value
  uint8_t  static  motorRun = 0;
  
  if (!motorRun && motorRunning ) {
    currentIndex = 0;
    currentFilterCounter = 0;
    currentFilterSum = 0; 
    currentStorePhase = 0;
    currentStoreValue = 0;      
    currentStoreIndex = 0;          
    currentStoreCounter = 0;    
    currentAverage = 0;            
  }
  
  if (motorRun) {
    
    r.gateTime++;

    // 1. filtering    
    currentFilterSum += (ADCL & 0xFF) ;
    currentFilterSum += (ADCH & 3)<<8;
    currentFilterCounter ++;
    
    if (currentFilterCounter >= CurrentFilterSamples) {
        digitalWrite(ImpulsePin, HIGH);

        currentIndex++;  // check next index
        if (currentIndex >= CurrentValues) 
          currentIndex = 0;

             
        // 2. summing the current fifo samples
        currentAverage -= current[currentIndex];            // subscript value which goes out from the current fifo
        current[currentIndex] = currentFilterSum / (CurrentFilterSamples * CurrentResolution);  // new sample
        currentAverage += current[currentIndex];          // add new sample to 


        
        // 3. store 
        if ( !currentStorePhase ) {
          gateCurrentStore(current[currentIndex]);
          if (currentStoreIndex == CurrentStoreFastSamples) 
            currentStorePhase++;
        }  
        else  {
          if (currentStoreCounter >= CurrentStoreAverages) {
            gateCurrentStore(currentStoreValue/CurrentStoreAverages);
            currentStoreCounter = 0;
            currentStoreValue   = 0;
          }
          else  {
            currentStoreValue += current[currentIndex];
            currentStoreCounter++; 
          }
        }

        
          
        // overcurrent motor off   
        if (    ( currentAverage * CurrentResolution > currentAverageMax ) 
             || ( r.gateTime > r.gateTimeout) )
          motorOff();
          
        
        currentFilterSum = 0;
        currentFilterCounter = 0;
    }
  }  
  motorRun = motorRunning;
  digitalWrite(ImpulsePin, LOW);
//  digitalWrite(LimitPin, LOW);

}


void prints() {
/*  dprint("Led counter =");
  dprintHex(ledCounter,HEX);
  dprint(" ledPulseCount =");
  dprintHex(ledPulseCount,HEX);
  dprint(" ledOn =");
  dprintHex(ledOn,HEX);
  dprint(" ledOnTime =");
  dprintHex(ledOnTime,HEX);
  dprint(" ledPeriod =");
  dprintHex(ledPeriod,HEX);*/
  dprint(" modeTimeout =");
  dprint(modeTimeout);
  dprint(" miliseconds =");
  dprint(miliseconds);
  dprintln();  
}

//********************************* MODE ****************************************

void setBasicMode() {
  mode = ModeBasic;
  modeTimeout =  0;               // time for expire this mode
  ledPulse(0, 100 );              // stop pulsing
  button = 0;  
  dprintln("Set Basic mode");

//  prints();
}


void setLearnMode(uint8_t add) {
  mode = ModeLearn;
  modeTimeout =  ModeLearnTimeout;  // time for expire this mode
  ledPulse(255,10,600);           // infinite pulses 100ms, 1 s period
  if (add ) {
    if (function < CmdGateStop)
      function++;                 // set basic function 
  }    
  else 
    function = CmdGateOposite;  
    ledPulse(255,10,500);           // infinite pulses 100ms, 1 s period
//  ledPulse((function-CmdGateOposite+1),10,100);           // infinite pulses 100ms, 1 s period
    
  button = 0;  
  dprint("Set Learn Mode");
  dprint(" function=");
  dprintln(function);

//  prints();
  
}


void stopLearnMode(uint8_t succesfull) {
  if ( succesfull ) 
    ledPulse(1,1000);               // long pulse if stop succesfully 
  else
    ledPulse(0, 100, 500 );        // stop pulsing
  
  modeTimeout =  0;                 // time for expire this mode
  mode = ModeBasic;                 // back to basic mode
  button = 0;  
  dprintln("Stop Learn Mode");
  
//  prints();  
}

//********************************* LED *******************************************

void ledPulse(uint8_t pulses, uint16_t pulseOnTime, uint16_t pulsePeriod) {
  
  cli();
  ledCounter = 0;
  ledOnTime  = pulseOnTime;
  if (pulseOnTime >= pulsePeriod)
    ledPeriod  = pulseOnTime+1;
  else
    ledPeriod  = pulsePeriod;
  ledPulseCount  = pulses;
  digitalWrite(LedPin, 1);
  sei();
}  


void ledPulseWait(uint8_t pulses, uint16_t pulseOnTime, uint16_t pulsePeriod ) {

  ledPulse( pulseOnTime, pulsePeriod, pulses );   
  
  if (pulses < 255)                     // wait for end if not infinite mode
    while ( pulses | ledCounter ) ;
}    

//********************************* MOTOR ****************************************

// the setup routine runs once when you press reset:
void motorInit()
{
  pinMode(PwmOutputPin,OUTPUT);     // set signal output
  pinMode(MotorOnPin, OUTPUT);  
  
  digitalWrite(MotorOnPin, LOW);    // motor off
  digitalWrite(PwmOutputPin, LOW);  // pwm side motor off
  TCCR2B &= 0XF8;                   // set timer 2 divisor to 1/8 of osc 1Mhz for PWM frequency of 3906.250 Hz  
  TCCR2B |= 0X02;
}


// the setup routine runs once when you press reset:
bool motorOn( bool motorDir,  uint8_t motorCmd, uint8_t motorSpeed)
{
  if( motorOffTime )  return false;

   dprint("Motor On"); 

  if (motorDir == DirectionOpen)  {
    pinMode(PwmOpenPin, OUTPUT);        // setup open direction
    digitalWrite(PwmOpenPin, LOW);   
    pinMode(PwmClosePin, INPUT);       
   dprint(" Open"); 
  } 
  else {
    pinMode(PwmClosePin, OUTPUT);       // setup close direction
    digitalWrite(PwmClosePin, LOW);   
    pinMode(PwmOpenPin, INPUT);             
    dprint(" Close"); 
  }   
  
  TCCR2A = 0x23;                        // setup PwmOutput
  OCR2B  = motorSpeed;
  digitalWrite(MotorOnPin, HIGH);       // motor ON
  
  memset((void*)current, 0, sizeof(current)); // clear current buffer
  memset((void*)r.gateCurrent, 0, sizeof(r.gateCurrent)); // clear current buffer
  currentAverage = 0;
  r.gateTime = 0; 
  ADMUX  = 0xC0 | CurfAnalog;           // ADC source is motor current sensor  
  motorDirection = motorDir;
  motorRunning = 1;                     // system variable motor

  dprintln(); 
  return true;
} 

  
void motorOff() {
  digitalWrite(MotorOnPin,  LOW);  // motor off  
  digitalWrite(PwmClosePin, LOW);  // right side motor off  
  digitalWrite(PwmOpenPin,  LOW);  // left side motor off
 
  TCCR2A = 0; 
  ADMUX  = 0xC0 | BatAnalog;       // ADC source is battery
  
  dprintln("Motor Off"); 
  motorRunning = 0;
  motorOffTime = MotorOffTime;    // must be switched off for some time
  
}

//********************************* INITS ****************************************

void adcInit() {
  ADMUX  = 0XC0 | BatAnalog;    // [7:6] 3=REF 1.1V, 1=REF AVCC [5] ADLAR (left adjust), [3:0]MUX => ADC(6)=CurfAnalog, ADC(7)=BatAnalog,    
  ADCSRB = 0x00;          // [2:0] Timer1 oveflow - ADC trigger //05
  ADCSRA = 0xE6;          // [7] ADEN, [5] autotrigger, [2:0] division factor = 64
}

// initialize variables and RAM structure
void variableInit() {
  uint8_t i, devices;
  
  memset((void*)&r, 0, sizeof(r));                     // clear ram structure
  r.gateOpenTime = 64000;
  r.gateTimeout  = 64000;
  eepromr((uint32_t)(&(pee->vbatMin)), r.vbatMin, sizeof(r.vbatMin) );  
  eepromr((uint32_t)(&(pee->vbatMax)), r.vbatMax, sizeof(r.vbatMax) );  
  r.vbatMin = (r.vbatMin + 50)*1000 / 33;
  r.vbatMax = (r.vbatMax + 50)*1000 / 33;  
   
}
// initialize IO pins
void pinsInit() {
  pinMode(ButtonPin, INPUT_PULLUP);
  // initialize the digital pin as an output.
  pinMode(LedPin, OUTPUT);
  digitalWrite(LedPin, HIGH);     
}

//********************************* SETUP *******************************************

// the setup routine runs once when you press reset:
void setup() {
  uint32_t serialn;
  
  pinsInit();
  variableInit();  // initialize variables
  
  
  
  D(Serial.begin(57600);)
  // prints title with ending line break
  dprint("Ovladani brany 2");  
  if (!rf22.init()) {    
    dprintln("  rf BAD");
  } 
  else {
    eepromr((uint32_t)(&(pee->device[0].serialn)), serialn, sizeof(serialn));  
    rf22.setThisAddress(serialn & 0xFF); 
    ledPulse(1,200, 300);
    dprintln("  rf OK");
  }
  
  adcInit ();
  motorInit ();
  intInit ();

}

//********************************* LOOP *********************************************

// the loop routine
void loop() {

  
  static int8_t index = -1;
  static int16_t adc = -1;

  if (motorRunning)
    if (index != currentIndex ) {
      index = currentIndex;
      dprint("I=");
      dprint(current[currentIndex]);
      dprint(" index=");
      dprint(currentIndex);
      dprint(" mt=");
      dprintln(r.gateTime);      
  }   
  else {
    cli();
    adc =  (ADCL & 0xFF);
    adc += (ADCH & 3)<<8;
    sei();

    if (adc <  r.vbatMin )
      digitalWrite(ChargePin, HIGH);
           
    if (adc >  r.vbatMax )
      digitalWrite(ChargePin, LOW);
  } 


   
  if ( mode == ModeLearn && modeTimeout==0 )
    setBasicMode();

  
    
  // button push actions
  switch (button) {
    
    case ButtonExtra: 
      dprintln("Button Extra");
//      ledPulse(1,2000);        // long pulse for remove remotes succesfully 
      removeAllDevices();
      button = 0;
      break;
      
    case ButtonLong: 
      dprintln("Button Long");
 //     ledPulse(1,1000);               // long confirm pulse
      sendCommand(0xFF,CmdRegister);    // send Register
      registerTimeout = RegisterTimeout;
      button = 0;         
      break;
      
    case ButtonShort:          // register new remote
      switch (mode) {

        case ModeLearn:        // add one to  function value
          setLearnMode(1);     // restart learn mode with function++
          break;

        case ModeBasic:        // start learn mode - wait for Register packet
          setLearnMode();
          break;
          
        default:  
          button = 0;
          break;
      }  
          
    default : 
 //     button = 0;
      break;
  }    

  
  // check if remote message is available 
  if (rf22.available())
  { 

    //  message has been received
    len = BufferSize;
    if ( rf22.recvfrom(&b.buf[0], &len, &from, &to, &id, &flags) )  // transfering to buffer
    {
      dprintln("Received packet.");
      receivePacket();      
    }  
    else 
      dprintln("Received bad packet.");
      

  }

  // motor start stop
  switch (motorCmd) {

    case CmdGateOposite:  
    case CmdWingOposite:  
       if (motorRunning) {
          motorOff();
          motorCmd = 0;
       }
       else    
         if ( motorOn(!motorDirection, motorCmd, MotorHighSpeed) )
           motorCmd = 0;
      break;
      
      case CmdGateOpen:  
      case CmdWingOpen:        
        if ( motorRunning) {
          if (motorDirection == DirectionOpen) 
            motorCmd = 0;
          else
            motorOff();
        }  
        else
          if ( motorOn(DirectionOpen,motorCmd, MotorHighSpeed) )
            motorCmd = 0;
        break;
        
      case CmdGateClose:  
      case CmdWingClose:  
        if ( motorRunning) {
          if (motorDirection == DirectionClose) 
            motorCmd = 0;
          else
            motorOff();
        }  
        else
          if ( motorOn(DirectionClose, motorCmd, MotorHighSpeed) )
            motorCmd = 0;
        break;
        
     case CmdGateStop:  
        motorOff();
        motorCmd = 0;
        break;
                
     default:
        motorCmd = 0;
  }


#ifdef   DEBUG
  // test only
 /*     dprint(" mt=");
      dprintln(r.gateTime);
 */     
  
# endif
  
}



/*
  else {


      adcf = 25.3*adc/1024;

      dprint(" ADC=");
      dprint(adcf);
      dprint(" ADC=");
      dprintHex(adc,HEX);
      dprint("V  mt=");
      dprintln(motorTime);
  }
  */

