/*
 * In the original BSD license, both occurrences of the phrase "COPYRIGHT
 * HOLDERS AND CONTRIBUTORS" in the disclaimer read "REGENTS AND CONTRIBUTORS".
 *
 * Here is the license template:
 *
 * Copyright (c) 2011, Johan Thelin <johan@thelins.se>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 * Neither the name of Johan Thelin nor the names of its contributors may be
 * used to endorse or promote products derived from this software without
 * specific prior written permission.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "robot.h"

#include <QPainterPath>
#include <math.h>

Robot::Robot(QObject *parent)
	: QObject(parent)
{
	// All is reset in the reset method called first
}

void Robot::step(QPixmap *pixmap)
{
	QPointF start = position();
	setPosition(position()+movementChange());
	QPointF end = position();
	setDirection(direction()+directionChange());

	if (m_penActive)
	{
		QPainter p(pixmap);
		p.setRenderHint(QPainter::Antialiasing);
		p.setPen(Qt::red);
		p.drawLine(start, end);
	}

	updateSensors(*pixmap);
}

void Robot::drawOverlay(QPainter &painter)
{
	QPainterPath path;

	path.addRect(QRectF(-QPointF(10, 20), QSizeF(20, 40)));
	path.addEllipse(QPointF(0, 15), 2, 2);

	painter.setPen(Qt::black);
	painter.setBrush(Qt::lightGray);

	painter.translate(position());
	painter.rotate(direction());

	painter.drawPath(path);
}

bool Robot::brightForward() const
{
	return m_brightForward;
}

bool Robot::penActive() const
{
	return m_penActive;
}

qreal Robot::leftEngineSpeed() const
{
	return m_leftEngineSpeed;
}

qreal Robot::rightEngineSpeed() const
{
	return m_rightEngineSpeed;
}

void Robot::reset(const QPixmap &pixmap)
{
	setPenActive(true);
	setLeftEngineSpeed(0);
	setRightEngineSpeed(0);
	setDirection(270);
	setPosition(QPointF(pixmap.width()/2, pixmap.height()/2));
	updateSensors(pixmap);
}

void Robot::setPenActive(bool pa)
{
	if (pa != m_penActive)
	{
		m_penActive = pa;
		emit penActiveChanged(m_penActive);
	}
}

void Robot::setLeftEngineSpeed(qreal s)
{
	s = qBound(-1.0, s, 1.0);

	if (s != m_leftEngineSpeed)
	{
		m_leftEngineSpeed = s;
		emit leftEngineSpeedChanged(m_leftEngineSpeed);
	}
}

void Robot::setRightEngineSpeed(qreal s)
{
	s = qBound(-1.0, s, 1.0);

	if (s != m_rightEngineSpeed)
	{
		m_rightEngineSpeed = s;
		emit rightEngineSpeedChanged(m_rightEngineSpeed);
	}
}

QPointF Robot::position() const
{
	return m_position;
}

void Robot::setPosition(const QPointF &p)
{
	m_position = p;
}

qreal Robot::direction() const
{
	return m_direction;
}

void Robot::setDirection(qreal d)
{
	while (d>360.0)
		d -= 360.0;

	m_direction = d;
}

void Robot::setBrightForward(bool value)
{
	if (value != m_brightForward)
	{
		m_brightForward = value;
		emit brightForwardChanged(m_brightForward);
	}
}

void Robot::updateSensors(const QPixmap &paper)
{
	QTransform t;
	t.rotate(direction());
	QPointF sensorPos = position() + t.map(QPointF(0, 15));

	setBrightForward(qGray(paper.toImage().pixel(sensorPos.toPoint())) > 20);
}

QPointF Robot::movementChange() const
{
	QTransform t;
	t.rotate(direction()+directionChange());

	return t.map(QPointF(0, (leftEngineSpeed() + rightEngineSpeed())/2));
}

qreal Robot::directionChange() const
{
	/*

	  <----+----> lwS
	    |    .
	    |    .
	    |   .
	    |   .
	    |a x----------------------
	    |-. ....  ) a
	    | .     ....
	    |.          ....
	    |.              ....
	  <----+----> rwR


	  */

	qreal tangens = (leftEngineSpeed()-rightEngineSpeed())/20.0;
	qreal angle = atan(tangens);

	return angle*180.0/M_PI;
}