/*! \file   ConstantMotor.H c++/ConstantMotor.H
    \brief  C++ Constant Velocity Motor Class Interface
    \author Zhengrong.Zang@gmail.com
	\date   2013.10.01
	
    Defines interface to control constant velocity motor
*/
// The contents of this file are subject to the Mozilla Public License
// Version 1.0 (the "License"); you may not use this file except in
// compliance with the License. You may obtain a copy of the License
// at http://www.mozilla.org/MPL/
// 
// Software distributed under the License is distributed on an "AS IS"
// basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See
// the License for the specific language governing rights and
// limitations under the License.

#ifndef _ConstantMotor_H_
#define _ConstantMotor_H_

#include <tm.h>

#include <c++/Sound.H>
#include <c++/Motor.H>
#include <c++/RotationSensor.H>

static Motor *motors;
static RotationSensor *velocitySensors;
static int mvelocity;
static int mhysteresis;

wakeup_t faster(wakeup_t data) {
	return abs(velocitySensors->velocity()) > mvelocity + mhysteresis;
}

wakeup_t slower(wakeup_t data) {
	return abs(velocitySensors->velocity()) < mvelocity - mhysteresis;
}

wakeup_t fasterOrSlower(wakeup_t data) {
	return faster(0) || slower(0);
}
	
class ConstantMotor {
public:
	static const int START_SPEED = 100;
	static const int FAST_VELOCITY = 150;
	static const int NORMAL_VELOCITY = 100;
	static const int SLOW_VELOCITY = 50;
	static const int HIGH_HYSTERSIS_FACTOR = 6;
	static const int NORMAL_HYSTERSIS_FACTOR = 8;
	static const int LOW_HYSTERSIS_FACTOR = 10;

	ConstantMotor(Motor *motor, RotationSensor *sensor, int velocity = NORMAL_VELOCITY, int hysteresisFactor = 0) {
		motors = motor;
		velocitySensors = sensor;
		mvelocity = velocity;
		mhysteresis = hysteresisFactor == 0 ? 0 : velocity / hysteresisFactor;
	}
	
	~ConstantMotor() {
		motors->stop();
	}
	
//	Motor *motor() const {
//		return motors;
//	}
	
	void velocity(int velocity) {
		mvelocity = velocity;
	}
	
	void hysteresisFactor(int hysteresisFactor) {
		mhysteresis = (hysteresisFactor == 0) ? 0 : (mvelocity / hysteresisFactor);
	}

	void hysteresis(int hysteresis) {
		mhysteresis = hysteresis;
	}

	void stop() {
		motors->stop();
	}
	
	void brake() {
		motors->brake();
	}
	
	void forward(int velocity, unsigned char speed) {
		motors->forward(speed);
		this->velocity(velocity);
	}
	
	void backward(int velocity, unsigned char speed) {
		motors->backward(speed);
		this->velocity(velocity);
	}
	
	const int velocity() const {
		return mvelocity;
	}
	
	const int hysteresis() const {
		return mhysteresis;
	}
	
	void run() {
		while (!shutdown_requested()) {
			if (faster(0)) {
				motors->stepUp(-1);
			}
			else if (slower(0)) {				
				motors->stepUp(1);
			}
			
			delay(1);
		}
		
	}
};

#endif // _ConstantMotor_H_