#include <Arduino.h>
#include <Wire.h>
//#include <Servo.h>
#ifdef __AVR_ATmega2560__
  // Arduino Mega
  #include <EEPROM.h>
#elif __SAM3X8E__
	// Arduino Due
#else
  #error "Wrong Arduino board - Choose Arduino Mega or Arduino Due (recommended) in Arduino IDE"
#endif
#include "config.h"



/*

#include "adcman.h"
//#define Console Serial
#define  pinBatteryVoltage A2
#define BATTERY_SW_OFF -1

enum {STATE_FORWARD, ACT_BATTERY_SW, STATE_ERROR, STATE_OFF,STATE_PERI_FIND};
//setCapture(pinBatteryVoltage, 1, false);

unsigned long nextTimeCheckBattery {millis() + 10000};
float batVoltage {4.5};
float batSwitchOffIfBelow {5.0};
int idleTimeSec {1};
char batMonitor {1};
 byte stateCurr {3};
 byte stateLast;
byte stateNext; 
float batGoHomeIfBelow {5.0};
char perimeterUse;
int batSwitchOffIfIdle;

 void setActuator(char type, int value){}    

void resetIdleTime(){
  if (idleTimeSec == BATTERY_SW_OFF){ // battery switched off?
    Serial.println(F("BATTERY switching ON again"));
    setActuator(ACT_BATTERY_SW, 1);  // switch on battery again (if connected via USB)
  }
  idleTimeSec = 0;
}

void setNextState(byte stateNew, byte dir);
void beep(int numberOfBeeps, boolean shortbeep);  
void loadSaveErrorCounters(boolean readflag);
void loadSaveRobotStats(boolean readflag);

void checkBattery(){
  if (millis() < nextTimeCheckBattery) return;
	nextTimeCheckBattery = millis() + 1000;  
  if (batVoltage < 4.0){      
    Serial.println("BATTERY NOT FOUND - PLEASE SWITCH ON BATTERY!"); 
  }
    
  if (batMonitor){
    if ((batVoltage < batSwitchOffIfBelow) && (idleTimeSec != BATTERY_SW_OFF)) {      
			Serial.print(F("Battery warning: triggered batSwitchOffIfBelow "));
			Serial.print(batVoltage);
			Serial.print(F("<"));
			Serial.println(batSwitchOffIfBelow);
      //addErrorCounter(ERR_BATTERY);      
			delay(2000); // avois corrupting EEPROM while this is also called when power is turned OFF
			//beep(2, true);      
			//loadSaveErrorCounters(false); // saves error counters
      //loadSaveRobotStats(false);    // saves robot stats
      idleTimeSec = BATTERY_SW_OFF; // flag to remember that battery is switched off
      Serial.println(F("BATTERY switching OFF"));
      
      //if (rmcsUse)  // tell Raspberry PI to shutdown
      //{
      //  rmcsSendOff(Console);
      //}
      
      setActuator(ACT_BATTERY_SW, 0);  // switch off battery                     
    }
    else if ((batVoltage < batGoHomeIfBelow) && (stateCurr == STATE_FORWARD) 
			&& (perimeterUse)) {    //UNTESTED please verify
      Serial.print(F("triggered batGoHomeIfBelow "));
			Serial.print(batVoltage);
			Serial.print(F("<"));
			Serial.println(batGoHomeIfBelow);
      beep(2, true);      
      setNextState(STATE_PERI_FIND, 0);
    }
  
	  // check if idle and robot battery can be switched off  
		if ( (stateCurr == STATE_OFF) || (stateCurr == STATE_ERROR) ) {      
			if (idleTimeSec != BATTERY_SW_OFF){ // battery already switched off?
				idleTimeSec ++; // add one second idle time
				if ((batSwitchOffIfIdle != 0) && (idleTimeSec > batSwitchOffIfIdle * 60)) {        
					Serial.println(F("triggered batSwitchOffIfIdle"));      
					beep(1, true);      
					loadSaveErrorCounters(false); // saves error counters
					loadSaveRobotStats(false);    // saves robot stats
					idleTimeSec = BATTERY_SW_OFF; // flag to remember that battery is switched off
					Serial.println(F("BATTERY switching OFF"));

                    //if(rmcsUse)  // Tell Raspberry PI to shutdown
                    //{
                    //   rmcsSendOff(Console);
                    //}
	
					setActuator(ACT_BATTERY_SW, 0);  // switch off battery               
				}
			}
		} else {
			resetIdleTime();          
		}
	}
}

*/

// battery------------------------------------------------------------------------------------------------
   // case SEN_BAT_VOLTAGE: ADCMan.read(pinVoltageMeasurement);  return ADCMan.read(pinBatteryVoltage); break;

void setup(){

  
  

//pinMode(pinBatteryVoltage, INPUT); 
//Serial.begin(9600);
//robot.setup();
}

    void loop(){

       //ADCMan.run();
    //readSensors();
    //checkBattery();
    //robot.loop();

    //Serial.print("123BATTERY NOT FOUND - PLEASE SWITCH ON BATTERY!");
		

ADCMan.setCapture(pinChargeCurrent, 1, true);
//Aktivierung des LaddeStrom Pins beim ADC-Managers      
  ADCMan.setCapture(pinMotorMowSense, 1, true);
  ADCMan.setCapture(pinMotorLeftSense, 1, true);
  ADCMan.setCapture(pinMotorRightSense, 1, true);
  ADCMan.setCapture(pinBatteryVoltage, 1, false);
  ADCMan.setCapture(pinChargeVoltage, 1, false);  
  ADCMan.setCapture(pinVoltageMeasurement, 1, false);  

    }