define PULSE_MIN 380 define PULSE_MAX 1140 define PULSE_MIN_H PULSE_MI

#define PULSE_MIN 380
#define PULSE_MAX 1140

#define PULSE_MIN_H (PULSE_MIN * 1.1)
#define PULSE_MAX_H (PULSE_MAX * 0.9)

#define PULSE_MIN_V (PULSE_MIN * 1.3)
#define PULSE_MAX_V (PULSE_MAX * 0.7)

#define WIDE_TIMEOUT 1000

int __count(char* string, ...)
{
  int res = 1;
                   va_list ap;
                   va_start(ap, string);
                   va_arg(ap,void*);
                   res += 1;
                   va_end(ap);
                   return res;
}
#define DBG1(a) Serial.println(a)
#define DBG2(a,b) Serial.print(a); Serial.println(b);
#define DBG3(a,b,c) Serial.print(a); Serial.print(b), Serial.println(c);
#define DBG4(a,b,c,d) Serial.print(a); Serial.print(b); Serial.print(c); Serial.println(d)
#define DBG5(a,b,c,d,e) Serial.print(a); Serial.print(b); Serial.print(c); Serial.print(d); Serial.println(e)
#define DBG6(a,b,c,d,e,f) Serial.print(a); Serial.print(b); Serial.print(c); Serial.print(d); Serial.print(e); Serial.println(f)

short horizontalAngle;
short verticalAngle;

inline void setVerticalAngle(byte angle)
{
  OCR1B = map(angle, 0, 180, PULSE_MIN_V, PULSE_MAX_V);
}

inline void setHorizontalAngle(byte angle)
{
  OCR1A = map(angle, 0, 180, PULSE_MIN_H, PULSE_MAX_H);
}

#include "scanlines.h"

void setup()
{
  Serial.begin(9600);

  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);

  DDRB = _BV(PORTB1) | _BV(PORTB2);
  TCCR1A = _BV(COM1A1) | _BV(COM1B1);
  TCCR1B = _BV(WGM13) | _BV(CS11);
  OCR1A = (PULSE_MIN_V + PULSE_MAX_V) / 2;
  OCR1B = (PULSE_MIN_H + PULSE_MAX_H) / 2;
  ICR1 = 10000;
  TCNT1H = 0;
  TCNT1L = 0;

  Scanlines::init(horizontalAngle, verticalAngle);
}

void loop()
{
  static bool firstCycle = true;

  static Direction generalDirection;
  static VerticalDirection verticalDirection;
  static HorizontalDirection horizontalDirection;


  static int sensor = 0;
  static int wideSensor = 0;
  static int wideSensorPrev = 0;

  sensor = analogRead(A1);
  wideSensorPrev = wideSensor;
  wideSensor = analogRead(A0);

  analogWrite(7, map(wideSensor, 0, 1024, 254, 0));
  analogWrite(6, map(sensor, 0, 1024, 254, 0));


  static WideSensorData wideSensorData;

  static unsigned int lastUpdateUp = millis();
  static unsigned int lastUpdateDown = millis();
  static unsigned int lastUpdateLeft = millis();
  static unsigned int lastUpdateRight = millis();

  unsigned int time = millis();

  if (wideSensor > 600)
  {
    if (wideSensorData.up != 180 && time - lastUpdateUp > WIDE_TIMEOUT)
    {
      Serial.println("UP timed out");
      wideSensorData.up = 180;
    }

    if (wideSensorData.down != 0 && time - lastUpdateDown > WIDE_TIMEOUT)
    {
      Serial.println("DOWN timed out");
      wideSensorData.down = 0;
    }

    if (wideSensorData.left != 0 && time - lastUpdateLeft > WIDE_TIMEOUT)
    {
      Serial.println("LEFT timed out");
      wideSensorData.left = 0;
    }

    if (wideSensorData.right != 180 && time - lastUpdateRight > WIDE_TIMEOUT)
    {
      Serial.println("RIGHT timed out");
      wideSensorData.right = 180;
    }
  }
  
  
  if (sensor < 600)
  {
    Serial.println("Got sensor signal, calling wideSensorData.reset()");
    wideSensorData.reset();
    return;
  }

  if (!firstCycle)
  {
      if (wideSensor < max(0, (wideSensorPrev - 200)))
      {
        DBG4("wideSensor ", wideSensor, ", prev ", wideSensorPrev);
        if (generalDirection == DIRECTION_HORIZONTAL || generalDirection == DIRECTION_BOTH)
        {

          if (horizontalDirection == DIRECTION_RIGHT)
          {
            DBG2("Better wide signal moving RIGHT, setting wideSensorData.left to ", horizontalAngle);
            wideSensorData.left = horizontalAngle;
            lastUpdateLeft = time;
          }

          else if (horizontalDirection == DIRECTION_LEFT)
          {
            DBG2("Better wide signal moving LEFT, setting wideSensorData.right to ", horizontalAngle);
            wideSensorData.right = horizontalAngle;
            lastUpdateRight = time;
          }

        }
        else if (generalDirection == DIRECTION_VERTICAL || generalDirection == DIRECTION_BOTH)
        {
          if (verticalDirection == DIRECTION_UP)
          {
            DBG2("Better wide signal moving UP, setting wideSensorData.down to ", verticalAngle);
            wideSensorData.down = verticalAngle;
            lastUpdateDown = time;
          }

          else if (verticalDirection == DIRECTION_DOWN)
          {
            DBG2("Better wide signal moving DOWN, setting wideSensorData.up to ", verticalAngle);
            wideSensorData.up = verticalAngle;
            lastUpdateUp = time;
          }

        }

      }

      if (wideSensor > wideSensorPrev + 200)
      {
        DBG4("wideSensor ", wideSensor, ", prev ", wideSensorPrev);
        if (generalDirection == DIRECTION_HORIZONTAL || generalDirection == DIRECTION_BOTH)
        {
          if (horizontalDirection == DIRECTION_RIGHT && horizontalAngle < wideSensorData.right)
          {
            DBG2("Worse wide signal moving RIGHT, setting wideSensorData.right to ", horizontalAngle);
            wideSensorData.right = horizontalAngle;
            lastUpdateRight = time;
          }

          if (horizontalDirection == DIRECTION_LEFT && horizontalAngle > wideSensorData.left)
          {
            DBG2("Worse wide signal moving LEFT, setting wideSensorData.left to ", horizontalAngle);
            wideSensorData.left = horizontalAngle;
            lastUpdateLeft = time;
          }
        }
        else if (generalDirection == DIRECTION_VERTICAL || generalDirection == DIRECTION_BOTH)
        {
          if (verticalDirection == DIRECTION_UP && verticalAngle > wideSensorData.up)
          {
            DBG2("Worse wide signal moving UP, setting wideSensorData.up to ", verticalAngle);
            wideSensorData.up = verticalAngle;
            lastUpdateUp = time;
          }

          if (verticalDirection == DIRECTION_DOWN && verticalAngle < wideSensorData.down)
          {
            DBG2("Worse wide signal moving DOWN, setting wideSensorData.down to ", verticalAngle);
            wideSensorData.down = horizontalAngle;
            lastUpdateDown = time;
          }
        }
      }

  }
  else
  {
    wideSensorPrev = wideSensor;
  }

  Scanlines::calculate(horizontalAngle, verticalAngle, wideSensorData, generalDirection, horizontalDirection, verticalDirection);

  setVerticalAngle(verticalAngle);
  setHorizontalAngle(horizontalAngle);

  firstCycle = false;
  delay(3);
}