#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;
}

}