/*-----------------------------------------------------------------------
 * Read bytes
 */
int  i2c_read(uchar chip, uint addr, int alen, uchar *buffer, int len)
{
	int shift;
	PRINTD("i2c_read: chip %02X addr %02X alen %d buffer %p len %d\n",
		chip, addr, alen, buffer, len);

#ifdef CFG_I2C_EEPROM_ADDR_OVERFLOW
	/*
	 * EEPROM chips that implement "address overflow" are ones
	 * like Catalyst 24WC04/08/16 which has 9/10/11 bits of
	 * address and the extra bits end up in the "chip address"
	 * bit slots. This makes a 24WC08 (1Kbyte) chip look like
	 * four 256 byte chips.
	 *
	 * Note that we consider the length of the address field to
	 * still be one byte because the extra address bits are
	 * hidden in the chip address.
	 */
	chip |= ((addr >> (alen * 8)) & CFG_I2C_EEPROM_ADDR_OVERFLOW);

	PRINTD("i2c_read: fix addr_overflow: chip %02X addr %02X\n",
		chip, addr);
#endif

	/*
	 * Do the addressing portion of a write cycle to set the
	 * chip's address pointer.  If the address length is zero,
	 * don't do the normal write cycle to set the address pointer,
	 * there is no address pointer in this chip.
	 */
	send_start();
	if(alen > 0) {
		if(write_byte(chip << 1)) {	/* write cycle */
			send_stop();
			PRINTD("i2c_read, no chip responded %02X\n", chip);
			return(1);
		}
		shift = (alen-1) * 8;
		while(alen-- > 0) {
			if(write_byte(addr >> shift)) {
				PRINTD("i2c_read, address not <ACK>ed\n");
				return(1);
			}
			shift -= 8;
		}
		send_stop();	/* reportedly some chips need a full stop */
		send_start();
	}
	/*
	 * Send the chip address again, this time for a read cycle.
	 * Then read the data.  On the last byte, we do a NACK instead
	 * of an ACK(len == 0) to terminate the read.
	 */
	write_byte((chip << 1) | 1);	/* read cycle */
	while(len-- > 0) {
		*buffer++ = read_byte(len == 0);
	}
	send_stop();
	return(0);
}

/*-----------------------------------------------------------------------
 * Write bytes
 */
int  i2c_write(uchar chip, uint addr, int alen, const uchar *buffer, int len)
{
	int shift, failures = 0;

	PRINTD("i2c_write: chip %02X addr %02X alen %d buffer %p len %d\n",
		chip, addr, alen, buffer, len);

	send_start();
	if(write_byte(chip << 1)) {	/* write cycle */
		send_stop();
		PRINTD("i2c_write, no chip responded %02X\n", chip);
		return(1);
	}
	shift = (alen-1) * 8;
	while(alen-- > 0) {
		if(write_byte(addr >> shift)) {
			PRINTD("i2c_write, address not <ACK>ed\n");
			return(1);
		}
		shift -= 8;
	}

	while(len-- > 0) {
		if(write_byte(*buffer++)) {
			failures++;
		}
	}
	send_stop();
	return(failures);
}

int fsa9480_i2c_write(u8 addr, u8 data)
{
	u8 chip = 0x4A;


	send_start();
	if (write_byte(chip << 1)) 	/* write cycle */
	{
		send_stop();
//		dprintf(INFO,"i2c_write, no chip responded %02X\n", chip);
		return(1);
	}
	if (write_byte(addr)) 
	{
//		dprintf(INFO,"i2c_write, address not <ACK>\n");
		return(1);
	}
	
	if (write_byte(data)) 
	{
//		dprintf(INFO,"i2c_write, data not <ACK>\n");
		return(1);
	}
	send_stop();

	return 0;	
}

void i2c_tx_handler(){
    //debugNum(1);
    switch(ic_ptr->status){
        case(I2C_STARTED):
            load_i2c_data(); //start handled same way as sending data - address should already be loaded.
            break;
        case(I2C_MASTER_SEND):
            ic_ptr->nack = SSPCON2bits.ACKSTAT;
            if((SSPCON2bits.ACKSTAT == 1) || check_if_send_stop()){ //ack not received or it should stop naturally
                //ic_ptr->outbufind--; //Resend last byte
                send_stop();
            }
            else{
                if(load_i2c_data() == 1) { //WCOL bit set
                    SSPCON1bits.WCOL = 0;
                    send_stop();
                }
            }
            break;
        case(I2C_STOPPED): //stop
            ic_ptr->status = I2C_IDLE;
            if(!ic_ptr->nack){
                if(ic_ptr->read_after_write){
                    i2c_master_recv(ic_ptr->addr); //send a request for data (either ack or data)
                }
                else{
#ifdef MASTER_PIC
                    if(colorSensorInitStage <= INT_CLEAR){
                        FromI2CInt_sendmsg(0, MSGT_COLOR_SENSOR_INIT, (void*) 0);
                    }
#endif
                    ic_ptr->read_after_write = 1; //reset to 1 just in case
                }
            }
            else{
                char error[6];
                if(ic_ptr->addr == SENSOR_ADDR){ //sensor pic
                    generateSensorPICDetectionError(error, sizeof error, UART_COMM);
                }
                else{//motor pic
                    generateMotorPICDetectionError(error, sizeof error, UART_COMM);
                }
                FromI2CInt_sendmsg(sizeof error, MSGT_I2C_MASTER_SEND_FAILED, error);
            }
            break;
        default:
            break;
    }
}

/*
 * This function does the actual read of the EEPROM. It needs the buffer into which the
 * read data is copied, the size of the EEPROM being read and the buffer size
 */
int read_eeprom(char *buffer, int eeprom_size, int size)
{
	int	i = 0, err;

	send_start();
	send_byte(W_HEADER);
	recv_ack();

	/* EEPROM with size of more than 2K need two byte addressing */
	if (eeprom_size > 2048) {
		send_byte(0x00);
		recv_ack();
	}

	send_start();
	send_byte(R_HEADER);
	err = recv_ack();
	if (err == -1)
		return err;

	for (i = 0; i < size; i++) {
		*buffer++ = recv_byte();
		send_ack();
	}

	/* Note : We should do some check if the buffer contains correct information */

	send_stop();
}

void module_cleanup(module_info * mod) {
    layout *curlay = NULL;
    char temp[8] = { 0 };
    int i = 0;
    button *btn = NULL;
    bool ret = false;

    /*Send command to stop test */
    send_stop(mod);

    if(is_camera_module(mod)) {
        usleep(500 * 1000);
        cam_stop_preview();
    }

    pthread_kill(g_module_tid, SIGUSR1);

    if(strstr(mod->config_list[KEY_PARAMETER].c_str(), "magnetic") != NULL) {
        curlay = acquire_cur_layout();
        for(i = 0; i < ROUND_ANGLE; i++) {
            snprintf(temp, sizeof(temp), "%d", i);
            if(curlay != NULL) {
                ret = curlay->delete_btn_by_name(temp);
                if(!ret) {
                    ALOGE("Fail to delete button %s", temp);
                }
            }
        }
        release_cur_layout();
    }
}

int i2c_master_tx_rx(uint8_t addr, uint8_t *tx_data, int tx_len, uint8_t *rx_data, int rx_len)
{
	int i;

	I2C0_SA = addr; // Set the slave addr

	I2C0_CNT = tx_len & 0xFF; // Set the tx data count
	I2C0_CON |= (1 << 9) | (1 << 10); // Set the Master Tx mode
	if (send_start()) I2C_QUIT_OP; // Trigger by sending Start
	for (i = 0; i < tx_len; i++) // Send Data
	{
		if (send_data(tx_data[i])) I2C_QUIT_OP;
	}
	while (!(I2C0_IRQSTATUS & I2C_ST_AR)) // Wait for ready for accessing registers after the tx complete
		;
	I2C0_IRQSTATUS |= I2C_ST_AR;
	I2C0_CNT = rx_len & 0xFF; // Set the rx data count
	I2C0_CON &= ~(1 << 9); // Set the Master Rx mode - note master is already set
	if (send_restart()) I2C_QUIT_OP; // Trigger by sending Start again
	for (i = 0; i < rx_len; i++) // Receive Data
	{
		if (recv_data(&rx_data[i])) I2C_QUIT_OP;
	}
	send_stop(); // Done, so Stop
	return 0;
}

irom i2c_error_t i2c_reset(void)
{
	int current;

	if(!i2c_flags.init_done)
		return(i2c_error_no_init);

	state = i2c_state_stop_send;

	send_stop();

	// if someone is holding the sda line, simulate clock cycles
	// to make them release it

	for(current = i2c_config_idle_timeout; current > 0; current--)
	{
		if(sda_is_set())
			break;

		clear_scl();
		delay();
		set_scl();
		delay();
	}

	if(!sda_is_set())
		return(i2c_error_sda_stuck);

	if(!scl_is_set())
		return(i2c_error_bus_lock);

	state = i2c_state_idle;

	return(i2c_error_ok);
}

/*-----------------------------------------------------------------------
 * Send a reset sequence consisting of 9 clocks with the data signal high
 * to clock any confused device back into an idle state.  Also send a
 * <stop> at the end of the sequence for belts & suspenders.
 */
static void send_reset(void)
{
#ifdef	CONFIG_MPC8260
	volatile ioport_t *iop = ioport_addr((immap_t *)CFG_IMMR, I2C_PORT);
#endif
#ifdef	CONFIG_8xx
	volatile immap_t *immr = (immap_t *)CFG_IMMR;
#endif
	int j;

	I2C_SCL(1);
	I2C_SDA(1);
#ifdef	I2C_INIT
	I2C_INIT;
#endif
	I2C_TRISTATE;
	for(j = 0; j < 9; j++) {
		I2C_SCL(0);
		I2C_DELAY;
		I2C_DELAY;
		I2C_SCL(1);
		I2C_DELAY;
		I2C_DELAY;
	}
	send_stop();
	I2C_TRISTATE;
}

void zmq::socket_base_t::stop ()
{
    //  Called by ctx when it is terminated (zmq_term).
    //  'stop' command is sent from the threads that called zmq_term to
    //  the thread owning the socket. This way, blocking call in the
    //  owner thread can be interrupted.
    send_stop ();
}

void navigate_path(){
	int bool;
	struct timespec wait;
	wait.tv_sec = 0;
	wait.tv_nsec = SLEEP_DURATION;
	reset_timer();
	set_direction(&current);
	set_distance(&current);
	send_direction(&current.current_destination.angle);
	send_distance(&current.current_destination.distance);
	while(running == 1){//Infinite loop until it reaches point or collision avoidance occurs.
		printf("%d\n", current.num);
		update_position(&current);
		send_position(&current.current_point);
		if(check(current.current_point, current.current_destination) == 1){
			if(check(current.current_point, current.ending_point) == 1){
				current.path[current.num] = current.current_destination;
				current.num++;
				count++;
				current.path = realloc(current.path, sizeof(position) * (current.num+1));
			    	if(current.path == NULL){
			    		printf("!!!!!\n");
			    		send_stop();
			    	}
				send_stop();
				bool = 0;
				free(current.path);
				free(route.path);
				break;
			}
			else{
			   current.path[current.num] = current.current_destination;
			   current.num++;
			   count++;
			   printf("%d\n", sizeof(position) * (current.num+1));
			   current.path = realloc(current.path, sizeof(position) * (current.num+1));
			    	if(current.path == NULL){
			    		printf("!!!!!\n");
			    		send_stop();
			    	}
			   current.current_destination = route.path[count];
			   bool = 1;
			   break;
			}
		}

static void
sig_composing_stop(XMPP_SERVER_REC *server, const char *dest)
{
	DATALIST_REC *rec;

	g_return_if_fail(IS_XMPP_SERVER(server));
	g_return_if_fail(dest != NULL);
	if ((rec = datalist_find(composings, server, dest)) != NULL)
		send_stop(server, dest, rec->data);
}

static
void process_user_packet(tc_user_t *u)
{
    unsigned char   frame[MAX_FRAME_LENGTH];

    if (send_stop(u)) {
        return;
    }

    while (true) {
        if (u->orig_frame->frame_len > MAX_FRAME_LENGTH) {
            tc_log_info(LOG_NOTICE, 0, " frame length may be damaged");
        }

        memcpy(frame, u->orig_frame->frame_data, u->orig_frame->frame_len);
        process_packet(u, frame);
        u->total_packets_sent++;
        u->orig_frame = u->orig_frame->next;


        if (send_stop(u)) {
            break;
        }
        tc_log_debug1(LOG_DEBUG, 0, "check resp waiting:%u",
                ntohs(u->src_port));
        if (!u->orig_frame->belong_to_the_same_req) {
            tc_log_debug2(LOG_DEBUG, 0, "user state:%d,port:%u",
                u->state.status, ntohs(u->src_port));
            if (u->state.status & SYN_CONFIRM) {
                tc_log_debug1(LOG_DEBUG, 0, "set resp waiting:%u",
                        ntohs(u->src_port));
                u->state.resp_waiting = 1;
            }
            break;
        } else {
            tc_log_debug1(LOG_DEBUG, 0, "the same req:%u",  ntohs(u->src_port));
        }
    }
}

/*-----------------------------------------------------------------------
 * Probe to see if a chip is present.  Also good for checking for the
 * completion of EEPROM writes since the chip stops responding until
 * the write completes (typically 10mSec).
 */
int i2c_probe(uchar addr)
{
	int rc;

	/*
	 * perform 1 byte write transaction with just address byte
	 * (fake write)
	 */
	send_start();
	rc = write_byte ((addr << 1) | 0);
	send_stop();

	return (rc ? 1 : 0);
}

void compare_tile(){
	if((int)(current.current_point.lon) != (int)(current.path[current.num-1].lon)
	|| (int)(current.current_point.lat) != (int)(current.path[current.num-1].lat))
	{
	current.path[current.num] = current.current_point;
	current.path[current.num].angle = current.current_destination.angle;
	current.num++;
	current.path = realloc(current.path, sizeof(position) *
	(current.num+1));
    	if(current.path == NULL){
    		printf("!!!!!\n");
    		send_stop();
    	}
	}
}

int i2c_master_tx(uint8_t addr, uint8_t *data, int len)
{
	int i;

	I2C0_SA = addr; // Set the slave addr

	I2C0_CNT = len & 0xFF; // Set the data count
	I2C0_CON |= (1 << 9) | (1 << 10); // Set the Master Tx mode
	if (send_start()) I2C_QUIT_OP; // Trigger by sending Start
	for (i = 0; i < len; i++)
	{
		if (send_data(data[i])) I2C_QUIT_OP;
	}
	send_stop(); // Done, so Stop
	return 0;
}

irom i2c_error_t i2c_send(int address, int length, const uint8_t *bytes)
{
	int current;
	i2c_error_t error;
	bool_t ack;

	if(!i2c_flags.init_done)
		return(i2c_error_no_init);

	if(state != i2c_state_idle)
		return(i2c_error_invalid_state_not_idle);

	state = i2c_state_header_send;

	if((error = send_header(address, i2c_direction_send)) != i2c_error_ok)
		return(error);

	for(current = 0; current < length; current++)
	{
		state = i2c_state_data_send_data;

		if((error = send_byte(bytes[current])) != i2c_error_ok)
			return(error);

		state = i2c_state_data_send_ack_receive;

		if((error = receive_ack(&ack)) != i2c_error_ok)
			return(error);

		state = i2c_state_data_send_ack_received;

		if(!ack)
			return(i2c_error_data_nak);
	}

	state = i2c_state_stop_send;

	if((error = send_stop()) != i2c_error_ok)
		return(error);

	state = i2c_state_idle;

	return(i2c_error_ok);
}

SendControlStatus
DataLink::send_control(const DataSampleHeader& header, ACE_Message_Block* message)
{
  DBG_ENTRY_LVL("DataLink", "send_control", 6);

  TransportSendControlElement* const elem =
    TransportSendControlElement::alloc(1, // initial_count
                                       GUID_UNKNOWN, &send_response_listener_,
                                       header, message, send_control_allocator_);
  if (!elem) return SEND_CONTROL_ERROR;

  send_response_listener_.track_message();

  RepoId senderId(header.publication_id_);
  send_start();
  send(elem);
  send_stop(senderId);

  return SEND_CONTROL_OK;
}

/*-----------------------------------------------------------------------
 * Send a reset sequence consisting of 9 clocks with the data signal high
 * to clock any confused device back into an idle state.  Also send a
 * <stop> at the end of the sequence for belts & suspenders.
 */
static void send_reset(void)
{
	int j;

	I2C_SCL(1);
	I2C_SDA(1);
#ifdef	I2C_INIT
	I2C_INIT;
#endif
	I2C_TRISTATE;
	for(j = 0; j < 9; j++) {
		I2C_SCL(0);
		I2C_DELAY;
		I2C_DELAY;
		I2C_SCL(1);
		I2C_DELAY;
		I2C_DELAY;
	}
	send_stop();
	I2C_TRISTATE;
}

/*-----------------------------------------------------------------------
 * Send a reset sequence consisting of 9 clocks with the data signal high
 * to clock any confused device back into an idle state.  Also send a
 * <stop> at the end of the sequence for belts & suspenders.
 */
static void send_reset(void)
{
	I2C_SOFT_DECLARATIONS	/* intentional without ';' */
	int j;

#ifdef	I2C_INIT
	I2C_INIT;
#endif
	I2C_SCL(1);
	I2C_SDA(1);
	I2C_TRISTATE;
	for(j = 0; j < 9; j++) {
		I2C_SCL(0);
		I2C_DELAY;
		I2C_DELAY;
		I2C_SCL(1);
		I2C_DELAY;
		I2C_DELAY;
	}
	send_stop();
	I2C_TRISTATE;
}