/*! \file   CurveSensor.H c++/CurveSensor.H
    \brief  C++ Curve Sensor Class Interface
    \author Zhengrong.Zang@gmail.com
	\date   2013.10.01
	
    Defines interface to rotation sensor on curve track
*/
// 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 _CurveSensor_H_
#define _CurveSensor_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 CurveSensor {
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;

	CurveSensor(Motor *motor, Sensor::Port sensorPort, int velocity = NORMAL_VELOCITY, int hysteresisFactor = 0) {
		motors = motor;
		velocitySensors = new RotationSensor(sensorPort);
		mvelocity = velocity;
		mhysteresis = hysteresisFactor == 0 ? 0 : velocity / hysteresisFactor;
	}
	
	~CurveSensor() {
		delete velocitySensors;
		motors->stop();
	}
	
	Motor *motor() const {
		return motors;
	}
	
	void velocity(int velocity) {
		mvelocity = velocity;
	}
	
	void hysteresis(int hysteresisFactor) {
		mhysteresis = (hysteresisFactor == 0) ? 0 : (mvelocity / hysteresisFactor);
	}
	
	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);
			}
		}
		
	}
};

#endif // _CurveSensor_H_