/*
 * StepperController.cpp
 *
 *  Created on: 27.10.2016
 *      Author: MolchMitDolch
 */

#include "StepperController.h"
#include <Arduino.h> 


StepperController::StepperController(const bool _Endschalter1, const bool _Endschalter2) : schrittmotor(200, 8, 11, 12, 13), enablePin1(9), enablePin2(10)
{
	axisEnable_Neg = _Endschalter1;
	axisEnable_Pos = _Endschalter2;
	generalEnableBit = false;
	
	stepsPerRevolution = 200;
	degPerStep = 360 / stepsPerRevolution;	// Einheit: Grad

	speed = 0;								// Einheit: RPM						// todo Konzept f�r Initialgeschwindigkeit �berlegen
	remainingSteps = 0;

  stepsPerUpdate = 10;
	distPerStep =  100;						// Einheit: mm/step					// todo Wert rechnerisch bestimmen
	maxSpeed    = 30;						// Einheit: RPM						// todo Wert empirisch   bestimmen


  step_delay = 0;   // da speed = 0, step_delay zuweisung nichtmöglich, daher =0...
  last_step_time = micros();

  limitSwitchStepCounter = 0;
  limitSwitchStepCounter_max = 10;
  
  pinMode(enablePin1, OUTPUT);
  pinMode(enablePin2, OUTPUT);
	schrittmotor.setSpeed(speed);	// Initialwert f�r Instanz... ggf unn�tig! todo nachlesen in stepper.h bzw. stepper.cpp
}



void StepperController::disableDirection(const unsigned int _direction)
{
	switch(_direction)
	{
		case 1:
		{
			axisEnable_Neg = false;
			break;
		}
		case 2:
		{
			axisEnable_Pos = false;
			break;
		}
		default:						// Falls undefinierter Parameter: Beide Achsen sprerren
		{
			axisEnable_Neg = false;
			axisEnable_Pos = false;
		}
	}
}



void StepperController::enableDirection(const unsigned int _direction)
{
	switch(_direction)
	{
		case 0:
		{
			axisEnable_Neg = true;
			axisEnable_Pos = true;
			break;
		}
		case 1:
		{
			axisEnable_Neg = true;
			break;
		}
		case 2:
		{
			axisEnable_Pos = true;
			break;
		}
		default:						// Falls undefinierter Parameter: Nichts freigeben
		{

		}
	}
}

void StepperController::setSpeed(const float _speed)
{
	/**
	 * Rechnung:
	 *
	 * RPM =                  Distance(mm)PerRevolution          /   Distance(mm)PerMinute
	 *
	 * RPM =       ( StepsPerRevolution * Distance(mm)PerStep )  /  ( mm/s     * 60s/minute )
	 *
	 * RPM = int(  ( stepsPerRevolution * distPerStep )          /    ( _speed *    60      )  )
	 */
  



	speed = int( (stepsPerRevolution * distPerStep)  /  (_speed * 60) );

speed = int(_speed); //<-------------------------

	if(speed > maxSpeed)
		speed = maxSpeed;


   step_delay = 60L * 1000L * 1000L / stepsPerRevolution / speed;

	schrittmotor.setSpeed(speed);

 // stepsPerUpdate anpasen ? <-----------------------------------------------------------------------------------------------------------------------------------------------------
}

void StepperController::turnTask(const float _distance)
{
	// Da diese Funktion keine direkte Drehung ausloest muss auch nicht geprueft werden ob die Anweisung legitim ist. Dies erfolgt in der update()-Fkt.

	/**
	 * Distance(mm) = steps * Distance(mm)PerStep
	 *
	 * --> steps = int(  Distance(mm) / Distance(mm)PerStep  )
	 */
  Serial.print("_distance: ");
  Serial.println(_distance);
  
//<---------------------------------------------------------------------------------------------------------------------------------
	remainingSteps = int(/*std::round*/(  _distance / /*distPerStep*/1  ));						// todo round() erst ab C++11, pr�fen ob Arduino IDE das unterst�tzt



}

void StepperController::stop()
{
	remainingSteps = 0;
}

void StepperController::generalEnable(const bool _status)
{
	if(_status == false)
	{   stop();		}
	
	generalEnableBit = _status;
}

void StepperController::update()
{
  

  
	if(remainingSteps != 0  &&  generalEnableBit == true)		// Eigendlich ist dieser fall schon in den inneren Klammern abgedeckt aber so wird f�r remainigSteps == 0 nur eine if-Abfrage gepr�ft
	{

    unsigned long now = micros();
    // move only if the appropriate delay has passed:
    if (now - last_step_time >= step_delay)
    {
      // get the timeStamp of when you stepped:
      last_step_time = now;
  



  
  		if(remainingSteps > 0 && axisEnable_Pos == true)
  		{
  			if(remainingSteps > stepsPerUpdate)
  			{
  			    turnSteps(stepsPerUpdate);
  			    remainingSteps -= stepsPerUpdate;
  			}
  			else
  			{
  			    turnSteps(remainingSteps);
  			    remainingSteps = 0;
  			}
  		}
      else if(remainingSteps < 0 && axisEnable_Neg == true)
  		{
  			if(remainingSteps < -stepsPerUpdate)
  			{
  			    turnSteps(-stepsPerUpdate);
  			    remainingSteps += stepsPerUpdate;
  			}
  			else
  			{
  			    turnSteps(remainingSteps);
  			    remainingSteps = 0;
  			}
  		}



      if(limitSwitchStepCounter >= limitSwitchStepCounter_max)
      {
        if(axisEnable_Pos == false)
        {
          axisEnable_Pos = true;
        }
    
        if(axisEnable_Neg == false)
        {
          axisEnable_Neg = true;
        }
    
        limitSwitchStepCounter = 0;
      }



    }
	}
}



void StepperController::turnSteps(const signed int _steps)
{
  if(axisEnable_Pos == false)
  {
    limitSwitchStepCounter += _steps;
  }

  if(axisEnable_Neg == false)
  {
    limitSwitchStepCounter -= _steps;
  }
  
	digitalWrite(enablePin1, HIGH);
	digitalWrite(enablePin2, HIGH);
	schrittmotor.step(_steps);
	digitalWrite(enablePin1, LOW);
	digitalWrite(enablePin2, LOW);
}