//////// PROGRAMACION ROBOT SECUENCIAS DE CONTINUIDAD 2V

#include <cc3.h>
#include <cc3_color_info.h>
#include <math.h>
#include <stdbool.h>
#include <stdlib.h>


#define SERVO_MIN 0
#define SERVO_MID 128
#define SERVO_MAX 255


static void robot_get_mean (cc3_color_info_pkt_t * t_pkt, int* speed, unsigned int* countdown);

/*********************************************************************
 *
 * MAIN
 *
 *********************************************************************/
int main (void)
{

  // initialize
  int speed = 0;
  unsigned int countdown = 0;
  cc3_color_info_pkt_t s_pkt;

  if (!cc3_camera_init ()) {
    cc3_led_set_state (0, true);
    exit (1);
  }

  cc3_camera_set_power_state (true);
  cc3_camera_set_resolution (CC3_CAMERA_RESOLUTION_LOW);
  cc3_pixbuf_frame_set_subsample (CC3_SUBSAMPLE_NEAREST, 2, 1);

  cc3_camera_set_colorspace(0);// Set colormode to YCrCb
  cc3_camera_set_auto_exposure(false);// autoexposure off
  cc3_camera_set_auto_white_balance(false);// auto whitebalance  off
  cc3_gpio_set_mode (0, CC3_GPIO_MODE_SERVO); // servo
  cc3_gpio_set_mode (1, CC3_GPIO_MODE_SERVO); // ir led power
  cc3_gpio_set_mode (2, CC3_GPIO_MODE_INPUT); // right sensor
  cc3_gpio_set_mode (3, CC3_GPIO_MODE_INPUT); // left sensor

  cc3_gpio_set_servo_position (0, SERVO_MID);

  // main loop
  while (true) {
	 // delay
     for (int i = 0; i < 10000; i++) ;
     // left sensor detected end
	 while (!cc3_gpio_get_value(3) && (speed < 0)) {
	    speed = abs(speed);
		cc3_gpio_set_servo_position (0, SERVO_MID + speed);
	 }
	 // right sensor detected end
	 while (!cc3_gpio_get_value(2) && (speed > 0)) {
	  	speed = -1*abs(speed);
	    cc3_gpio_set_servo_position (0, SERVO_MID + speed);
	 }
	 // set speed and time
	 if (countdown <= 0) {
		robot_get_mean (&s_pkt, &speed, &countdown);
		cc3_gpio_set_servo_position (0, SERVO_MID + speed);
	 }
     countdown--;
  }

  return 0;
}


/*********************************************************************
 *
 * ROBOT GET MEAN
 *
 *********************************************************************/

void robot_get_mean (cc3_color_info_pkt_t * s_pkt, int* speed, unsigned int* countdown)
{
  cc3_image_t img;
  img.channels = 3;
  img.width = cc3_g_pixbuf_frame.width;
  img.height = 1;               // image will hold just 1 row for scanline processing
  img.pix = malloc (3 * img.width);

    cc3_pixbuf_load ();

    if (cc3_color_info_scanline_start (s_pkt) != 0) {
      while (cc3_pixbuf_read_rows (img.pix, 1)) {
        cc3_color_info_scanline (&img, s_pkt);
      }
      cc3_color_info_scanline_finish (s_pkt);


     *speed = (s_pkt->mean.channel[0]-60)/5;
     *countdown = s_pkt->deviation.channel[2]*100;

    }
  free (img.pix);
}