#include "scanlines.h"
#include "Arduino.h"
#define H_STEP 1
#define V_STEP 10
#define H_WIDE_SENSOR_OFFSET 5
#define V_WIDE_SENSOR_OFFSET 5
namespace Scanlines
{
static HorizontalDirection hDirection = DIRECTION_RIGHT;
static VerticalDirection vDirection = DIRECTION_UP;
void init(short& horizontalAngle, short& verticalAngle)
{
horizontalAngle = 0;
verticalAngle = 0;
hDirection = DIRECTION_RIGHT;
vDirection = DIRECTION_UP;
}
VerticalDirection reverse(VerticalDirection dir)
{
return (dir == DIRECTION_UP) ? DIRECTION_DOWN : DIRECTION_UP;
}
HorizontalDirection reverse(HorizontalDirection dir)
{
return (dir == DIRECTION_LEFT) ? DIRECTION_RIGHT : DIRECTION_LEFT;
}
inline void angleStep(HorizontalDirection dir, short& angle)
{
angle += (dir == DIRECTION_LEFT) ? (-H_STEP) : (H_STEP);
}
inline void angleStep(VerticalDirection dir, short& angle)
{
angle += (dir == DIRECTION_DOWN) ? (-V_STEP) : (V_STEP);
}
void calculate(short& horizontalAngle, short& verticalAngle, WideSensorData& wideSensorData, Direction& dir, HorizontalDirection& hDir, VerticalDirection& vDir)
{
angleStep(hDirection, horizontalAngle);
dir = DIRECTION_HORIZONTAL;
if (horizontalAngle >= min(180, (wideSensorData.right + H_WIDE_SENSOR_OFFSET)) || horizontalAngle <= max(0, wideSensorData.left - H_WIDE_SENSOR_OFFSET))
{
hDirection = reverse(hDirection);
angleStep(vDirection, verticalAngle);
if (verticalAngle >= min(180, (wideSensorData.up + V_WIDE_SENSOR_OFFSET)) || verticalAngle <= max(0, wideSensorData.down - V_WIDE_SENSOR_OFFSET))
{
dir = DIRECTION_BOTH;
vDirection = reverse(vDirection);
}
}
hDir = hDirection;
vDir = vDirection;
}
}