#include "screen_photomode.h"
#include "button.h"
#include "tft.h"
#include "touch.h"
#include "pixy.h"
#include "pixy_helper.h"
#include "system.h"

static volatile bool pixy_connected = false;


static BUTTON_STRUCT b_back;
static TOUCH_AREA_STRUCT a_area;

static void b_back_cb(void* button) {
        gui_screen_back();
}

static POINT_STRUCT pixy_pos;
static POINT_STRUCT old_pos;
static void touchCB(void* touchArea, TOUCH_ACTION triggeredAction) {
	POINT_STRUCT p = touch_get_last_point();
	switch(triggeredAction) {
		case PEN_ENTER:
        case PEN_DOWN:
			old_pos = p;
            break;
        case PEN_MOVE:
		{
			int16_t deltaX = p.x - old_pos.x;
			int16_t deltaY = p.y - old_pos.y;
			old_pos=p;
			printf("%d %d\n",deltaX,deltaY);
			if(pixy_connected) {
				int16_t new_x = pixy_pos.x+deltaX*2;
				int16_t new_y = pixy_pos.y-deltaY*2;
				if(new_x<0) new_x=0;
				if(new_x>1000) new_x=1000;
				if(new_y<0) new_y=0;
				if(new_y>1000) new_y=1000;
				pixy_pos.x = new_x;
				pixy_pos.y= new_y;
			}
		}
        break;
		case PEN_UP:
		case PEN_LEAVE:
			printf("Leave/up\n");
			break;
		default: break;
        }

	
}


static void enter(void* screen) {
	tft_clear(WHITE);
	
	//Back button
	b_back.base.x1=10; //Start X of Button
        b_back.base.y1=210; //Start Y of Button
        b_back.base.x2=AUTO; //b_back.base.x1+160; //Auto Calculate X2 with String Width
        b_back.base.y2=AUTO; //Auto Calculate Y2 with String Height
        b_back.txtcolor=WHITE; //Set foreground color
        b_back.bgcolor=HEX(0xAE1010); //Set background color (Don't take 255 or 0 on at least one channel, to make shadows possible)
        b_back.font=0; //Select Font
        b_back.text="Back"; //Set Text (For formatted strings take sprintf)
        b_back.callback=b_back_cb; //Call b_back_cb as Callback
        gui_button_add(&b_back); //Register Button (and run the callback from now on)

	//Area test
        a_area.hookedActions =  PEN_DOWN | PEN_MOVE |  PEN_ENTER | PEN_UP | PEN_LEAVE;
        a_area.x1 = 0;
        a_area.y1 = 0;
        a_area.x2 = 317;
        a_area.y2 = 197;
        a_area.callback = touchCB;
        touch_register_area(&a_area);



	//Pixy stuff
        pixy_connected = (pixy_init()==0); //try to connect to pixy
	if(pixy_connected) {
		pixy_pos.x=pixy_pos.y=500;
	}
}

static void leave(void* screen) {
	gui_button_remove(&b_back);
	touch_unregister_area(&a_area);
}



static void update(void* screen) {

  //Note: The only way to detect that pixy has been disconnected is if a command fails. There's no pixy_is_connected method yet :'(

	if(!pixy_connected) { //Pixy not connected
		pixy_close(); //Ensure that all pixy resources are freed (failsafe)
		if(pixy_init()==0) { //try to connect to pixy
			pixy_connected=true;
			pixy_pos.x=pixy_pos.y=500;
			printf("pixy reinitialized\n");
		}
	}

	if(pixy_connected) {
		pixy_service(); //Send/receive event data from/to pixy failed

		pixy_render_full_frame(1,1);
	
		pixy_rcs_set_position(0,pixy_pos.x);
		pixy_rcs_set_position(1,pixy_pos.y);
	}
}


static SCREEN_STRUCT screen = {
	enter,
	leave,
	update
};


SCREEN_STRUCT* get_screen_photomode() {
	return &screen;
}