#include "misaligned_turnout.h"

#define OPTIMIZED_QUERIES
#define STRAIGHT_CHECK_ONLY

#ifdef OPTIMIZED_QUERIES
void misaligned_turnout_matcher(ModelRoot *model, MisalignedTurnoutFrame *frame,
                                int startOpIndex,
                                MisalignedTurnoutMatchSet *results) {
  int match_cntr = 0;

  int loop_bound0 = model->turnout_count;
  for (int i0 = 0; i0 < loop_bound0; i0++) {
    frame->turnout = model->turnouts[i0];
    frame->location = frame->turnout->straight;
    if (frame->location != NULL) {
      Segment *disconnected = ((Segment *)frame->turnout);
      uint8_t is_connected_to = 0;
      is_connected_to |= (frame->location->connected_to[0] == disconnected);
      is_connected_to |= (frame->location->connected_to[1] == disconnected);
      if (is_connected_to == 0) {
        frame->train = frame->location->train;
        if (frame->train != NULL) {
          results->matches[match_cntr].location = frame->location;
          results->matches[match_cntr++].train = frame->train;
        }
      }
    }
#ifndef STRAIGHT_CHECK_ONLY // Do not check the divergent direction, only the
                            // straight one
    frame->location = frame->turnout->divergent;
    if (frame->location != NULL) {
      Segment *disconnected = ((Segment *)frame->turnout);
      uint8_t is_connected_to = 0;
      is_connected_to |= (frame->location->connected_to[0] == disconnected);
      is_connected_to |= (frame->location->connected_to[1] == disconnected);
      if (is_connected_to == 0) {
        frame->train = frame->location->train;
        if (frame->train != NULL) {
          results->matches[match_cntr].location = frame->location;
          results->matches[match_cntr++].train = frame->train;
        }
      }
    }
#endif // simple example
  }

  results->size = match_cntr;
}

#else  // original queries

// pattern misalignedTurnout(location : Segment, train : Train) {
// 	Turnout(turnout);
// 	Segment.occupiedBy(location, train);
// 	Turnout.straight(turnout, location);
// 	neg find connected(location, turnout);
// }
// or {
//	Turnout(turnout);
//	Segment.occupiedBy(location, train);
//	Turnout.divergent(turnout, location);
//	neg find connected(location, turnout);
//}

// The private pattern below is used only by this query and is not part of the
// API

// private pattern connected(a : Segment, b : Segment) {
// 	Segment.connectedTo(a, b);
// }

typedef struct _ConnectedFrame {
  Segment *s1;
  Segment *s2;
} ConnectedFrame;

typedef struct _ConnectedMatch {
  Segment *s1;
  Segment *s2;
} ConnectedMatch;

typedef struct _ConnectedMatchSet {
  int size;
  ConnectedMatch matches[2];
} ConnectedMatchSet;

void connected_matcher(ModelRoot *model, ConnectedFrame *frame,
                       int startOpIndex, ConnectedMatchSet *results) {
  int match_cntr = 0;

  int loop_bound0 = frame->s1->connected_to_count;
  for (int i0 = 0; i0 < loop_bound0; i0++) {
    if (frame->s1->connected_to[i0] == frame->s2) {
      results->matches[match_cntr].s1 = frame->s1;
      results->matches[match_cntr++].s2 = frame->s2;
    }
  }

  results->size = match_cntr;
}

void misaligned_turnout_matcher(ModelRoot *model, MisalignedTurnoutFrame *frame,
                                int startOpIndex,
                                MisalignedTurnoutMatchSet *results) {
  int match_cntr = 0;

  int loop_bound0 = model->turnout_count;
  for (int i0 = 0; i0 < loop_bound0; i0++) {
    frame->turnout = model->turnouts[i0];
    frame->location = frame->turnout->straight;
    if (frame->location != NULL) {
      Segment *disconnected = ((Segment *)frame->turnout);
      ConnectedFrame connected_vars = {.s1 = disconnected,
                                       .s2 = frame->location};
      ConnectedMatchSet connected_match_set = {.matches = {NULL}, .size = 0};
      connected_matcher(model, &connected_vars, 0, &connected_match_set);
      if (connected_match_set.size == 0) {
        frame->train = frame->location->train;
        if (frame->train != NULL) {
          results->matches[match_cntr].location = frame->location;
          results->matches[match_cntr++].train = frame->train;
        }
      }
    }
  }
  for (int i0 = 0; i0 < loop_bound0; i0++) {
    frame->turnout = model->turnouts[i0];
    frame->location = frame->turnout->divergent;
    if (frame->location != NULL) {
      Segment *disconnected = ((Segment *)frame->turnout);
      ConnectedFrame connected_vars = {.s1 = disconnected,
                                       .s2 = frame->location};
      ConnectedMatchSet connected_match_set = {.matches = {NULL}, .size = 0};
      connected_matcher(model, &connected_vars, 0, &connected_match_set);
      if (connected_match_set.size == 0) {
        frame->train = frame->location->train;
        if (frame->train != NULL) {
          results->matches[match_cntr].location = frame->location;
          results->matches[match_cntr++].train = frame->train;
        }
      }
    }
  }

  results->size = match_cntr;
}
#endif // OPTIMIZED_QUERIES
