#include <opencv2/imgproc.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>

#include "LibCamera.h"

#include <wiringPi.h>
#include <wiringPiI2C.h>
#include "../ServoSet/Adafruit_PWMServoDriver.h"
#include "../ServoSet/servo.h"
#include "../ServoSet/util.h"

#include "Deal.h"
// #include "CVGraph.h"
#include "CBitmap.h"
#include "Card.h"
#include "Recognizer.h"
#include "files.h"


using namespace cv;
using namespace std;


void quit(const char * msg) {	// Fatal error routine
  printf("%s\n", msg);
  servoSleep();
}


void dealTo(int hand);

LibCamera cam;
LibcameraOutData frameData;

Recognizer rec;
const int width = 640;
const int height = 480;
const int iDelay = 150;


// Initializes the camera
void setupCamera() {
  int ret= cam.initCamera(width, height, formats::RGB888, 1, 0);
  if (ret) {
    printf("Error in initCamera, %d.\n", ret);
    quit("Leaving.\n");
  }

  ControlList controls_;
  int64_t frame_time = 1000000 / 30;
  controls_.set(controls::FrameDurationLimits, { frame_time, frame_time });
  cam.set(controls_);
  cam.startCamera();
  printf("Camera setup.\n");
}

int lastX;			// Last position of the X (azimuth) servo

/* *************** ************ *************/
int main() {
  uchar hands[52];

  /* ************** Define servo positions **************/
#define YDOWN 287		// lowest we go
#define YUP 308			// high enough to clear the hopper
#define Y2CLEAR 316		// high enough to clear obstructions

#define PARK    servoMoveTo(320, 316), delay(iDelay) // neutral position
#define CAMERAU  servoMoveTo(325, Y2CLEAR), delay(iDelay) // Up above camera box
#define CAMERAD  servoMoveTo(325, 287), delay(iDelay) // down to take picture
#define CARDU_CW   servoMoveTo(lastX=295, Y2CLEAR), delay(iDelay) // above the card stack, moving CW
#define CARDU_CCW  servoMoveTo(lastX=300, Y2CLEAR), delay(iDelay) // above stack, moving ccw
#define CARDD   servoMoveTo(lastX, YDOWN), delay(iDelay)	  // down do card stack
#define JOGY 310		// level at which we do a little jog
#define JOG     servoMoveTo(lastX, JOGY); delay(iDelay); servoMoveTo(lastX, JOGY-5); delay(2*iDelay)

#define HANDD   servoMoveTo(273, 297), delay(iDelay) // CW of the cards
#define HANDU   servoMoveTo(273, YUP), delay(iDelay)

#define SUCK    servoSetPin(2, 4096); delay(500);
#define UNSUCK  servoSetPin(2,0); delay(500);


  rec.readCards("cards/cards.bin"); // Read our stored card images
  for (int i=0; i == 0; )	      // Get the hand we will deal
    i = getHand("cards/Hands", hands);
  printHands(hands);

  setupCamera();		// Start the camera
  int i = wiringPiSetup();	// init the servos
  if (i < 0) {		// this usually happens if we don't have permission
    printf("wiringPiSetupGpio returned error %d\n", i);
    return 0;
  } else printf("wiringPi setup OK.\n");

  initPins();
  servoSetPin(2,0);		// sucker off
  servoSetMinMax(238, 381, YDOWN, 317); // set the limits of the servos
  servoBegin();

  while (true) {		// prime camera by taking first frame...
    bool flag = cam.readFrame(&frameData);
    if (!flag) continue;
    Mat im(height, width, CV_8UC3, frameData.imageData);
    imshow("Read Cards", im);
    (void) waitKey(500);
    break;
  }
  cam.returnFrameBuffer(frameData);
  HANDU;			// move to above the camera
  CARDU_CCW;			// then to above the card hopper, moving counterclockwise

  for (int j=0; j < 52; j++) { // Deal each card...
    //CARDU_CCW;
    CARDD;			// down to card hopper
    SUCK;
    JOG;			// shake a little
    CARDU_CW;		// up above hopper
    delay(iDelay);
    CAMERAU;		// over camera
    CAMERAD;		// down to camera

    Mat im;             // grab the image
    int iFrame = 0;
    while (true) {
      bool flag = cam.readFrame(&frameData);
      if (!flag) continue;
      im = Mat(height, width, CV_8UC3, frameData.imageData); // read card image
      imshow("Read Cards", im);
      (void) waitKey(10);
      if (iFrame++ < 5) {	// seems we have to do this 5 times to empty the buffers
	cam.returnFrameBuffer(frameData);
	continue;
      }
#if 1               // write card images for debugging
      char buff[60];
      sprintf(buff, "cards/imge%02d.jpg", j);
      waitKey(10);
      if (!imwrite(buff, im)) {
	printf("Error writing file %s\n", buff);
	quit("quitting.");
      } else printf("Wrote %s\n", buff);
#endif // 0
      break;
      cam.returnFrameBuffer(frameData);
    }
    int index = rec.recognize(im); // recognize the card.
    printf("%2d Recognized %s -> %c\n", j+1, cardName(index, false).c_str(), handName[hands[index]]);
    cam.returnFrameBuffer(frameData);
    dealTo(hands[index]);	// deal the card
  }
  PARK;				// move back to hopper

  cam.stopCamera();		// Done
  destroyAllWindows();
  cam.closeCamera();

}


/* Deal the card to the designated hand, 0 = N, 1 = E, 2 = S, 3 = W */
void dealTo(int hand) {
  // X locations of the piles for the four hands
  const int handX[] = { 378, 351, 267, 240 };
  if (hand < 0 || hand > 3) {
    printf("moveTo: illegal hand %d\n", hand);
    exit(0);
  }
  CAMERAU;
  if (hand <= 1) {
    servoMoveTo(handX[1], YUP);
    delay(iDelay);
    servoMoveTo(handX[1], YDOWN);
    if (hand == 0) {
      servoMoveTo(handX[0], YDOWN);
    }
  } else {
    servoMoveTo(handX[2], Y2CLEAR);
    servoMoveTo(handX[2], YDOWN);
    if (hand == 3) {
      servoMoveTo(handX[3], YDOWN);
    }
  }
  UNSUCK;
  servoMoveTo(handX[hand], Y2CLEAR);
  if (hand >= 2) {
    CARDU_CCW;
  } else {
    CARDU_CW;
  }
}