// Do not remove the include below
#include "Daypower_virtuelle_Endschalter.h"


//Vaiables////////////////////////////////////////////////////////
// set the number of steps of the motor

const int blinkPin = 13;
bool blinker = 0;

/*convert App data:
App: 								Genuino 101:
	STOP == 		2					STOP ==			50
	Rechtslauf == 	1					Rechtslauf ==	49
	Linkslauf == 	0					Linkslauf ==	48
	loop ==			4					Speed == 		51
*/


// create an instance of the stepper class
DaypowerStepper stepmotor(STEPS, IN1, IN2, IN3, IN4); // wndg groups 6-8 and 7-9 (!)

float scaleFrequency = .5; // test signal frequency (Hz)
FilterOnePole filterOneLowpassScale( LOWPASS, scaleFrequency ); // create a one pole (RC) lowpass filter
FilterOnePole filterOneLowpassDiff (LOWPASS, 0.2);

//Ableitung Diff(0.1,micros());	//Instanz zum Ableiten des letzten Messwertes, mit Anfangswert = 0.1(!=0), und Anfangszeit = millis();
FilterDerivative Diff;

Adafruit_INA219 ina219;
float current_mA = 0;
uint64_t prev_meas_millis = millis();
float Messwert;
//////////////////////////////////////////////////////////////////
//Flags zur Zustandsauswertung
virtuellerEndschalter eSchie;
bool goloop = false;
int richtung = 2;			//Richtung <0==links, >0==rechts

//////////////////////////////////////////////////////////////////

void setup() {
  // USB-VCP eröffnen
  Serial.begin(74880);

  BluetoothLESetup();          //Set up BT

  // set the speed of the motor in rpm (Runden pro Minute)
  stepmotor.setUserSpeed(12);	//Higher Speed for testing

  // Initialize the INA219.
  // By default the initialization will use the largest range (32V, 2A).  However
  // you can call a setCalibration function to change this range (see comments).
  ina219.begin();
  ina219.setCalibration_16V_400mA();
  // To use a slightly lower 32V, 1A range (higher precision on amps):
  //ina219.setCalibration_32V_1A();
  // Or to use a lower 16V, 400mA range (higher precision on volts and amps):
  //ina219.setCalibration_16V_400mA();

}

void loop(void)
{
	    int8_t bt_value = get_Bluetooth_values();
	    static int8_t bt_value_old = 0;

	    if (bt_value != -1 && bt_value_old != bt_value) { //Routine wenn kein Fehler und neuer Wert
	      Serial.print("Value true: ");
	      Serial.println(bt_value);
	      bt_value_old = bt_value;

	      switch(bt_value){
	      	  case 50: stepmotor.setVal(0); goloop = false;				break;		//STOP
	      	  case 49: richtung = 2; stepmotor.setVal(richtung);		break;		//Rechtslauf
	      	  case 48: richtung = -2; stepmotor.setVal(richtung);		break;		//Linkslauf
	      	  case 51: goloop = true;
//	      	  default:stepmotor.setUserSpeed(bt_value);		break;	//Geschwindigkeit setzen
	      }


	    }
	    else if (bt_value == -1) {							//Routine wenn Fehler
	      if (bt_value_old != bt_value) {
	        Serial.print("Value false: ");
	        Serial.println(bt_value);
	        bt_value_old = bt_value;
	      }
	    }

	    else {												// Routine wenn kein Fehler und kein neuer Wert

	    }

	    // Call Funktion to run Stepper;

	    stepmotor.run();



	    if ((millis() - prev_meas_millis) > MEASDELAY ){
	    	current_mA = ina219.getCurrent_mA();
//	    	Serial.println(current_mA);
	    	filterOneLowpassScale.input(current_mA);
	    	Messwert = filterOneLowpassDiff.input(Diff.input(filterOneLowpassScale.output()));
	    	Serial.print(Messwert);
	    	Serial.print(";\n");
	    }


	    //Zustandsauswertung
	    eSchie.checkZustand(Messwert);

	    //Auswertung
	    if(eSchie.getZustand()){	//Zustand abfragen und evtl reagieren
	    	if(goloop){				//Richtung drehen wenn goloop
	    		richtung = -1*richtung;
	    		stepmotor.setVal(richtung);
	    	}
	    	else{
	    		stepmotor.setVal(0);		//Motor stoppen
	    	}

	    }


}

//TODO:
//Stand 8.11.2016. Hält an aber Anfahrt in gleiche Richtung nicht möglich.
//Verzögerung im Stoppen ---> TP aggresiver machen für schnellere Reaktion


////////  Functions  ////////////////////////////////////////////////////////////////