#include <htc.h>
#include "i2c.h"
#include "rs232.h"
#include "svastara.h"

#define ADRESA_MAG (0x3C)

#ifndef _XTAL_FREQ
#define _XTAL_FREQ 4000000
#endif

void init_PIC(void)
{
	ANSEL = 0x00;
	ANSELH = 0x00;
	TRISA = 0x00;
	TRISC = 0x00;
	PORTA = 0x00;
	TRISB = 0xF0;
	SSPCON = 0b00111011;
}

void start(int x)
{
	for (int i=0; i<x; i++)
	{
		PORTC = 0x0F;
		__delay_ms(100);
		PORTC = 0x00;
		__delay_ms(100);
	}
}

int i2c_init(unsigned char ADRESA1, unsigned char ADRESA2, unsigned char ADRESA3)
{
	if (i2c_open(ADRESA1, I2C_WRITE)) { rs232_send_text("greska1\n"); return 0; }
	if (i2c_put_byte(ADRESA2) != 0) { rs232_send_text("greska2\n");  return 0; }
	if (i2c_put_byte(ADRESA3) ) {rs232_send_text("greska3\}"); return 0;}
	i2c_stop();
	return 1;
}


int read_i2c(char ADRESA, char adr_reg)
{
	char data;
	
	if(i2c_open(ADRESA, I2C_WRITE)) {rs232_send_text("error1\n"); return -1;}
	if(i2c_put_byte(adr_reg) !=0) {rs232_send_text("error2\n"); return -1;}
	
	if(i2c_open(ADRESA, I2C_READ)) {rs232_send_text("error3\n"); return -1;}
	data = i2c_get_byte(0);
	
	return data;	
}


float rezultat(char high, char low)
{
	char mask = 0x80;
	int high_int, low_int;
	float minus = 1, rez = 0;
	if(high&mask)
	{
		high^=0xFF;
		low^=0xFF;
		low+=0x01;
		minus = -1;
	}
	
    high_int = high<<8;
    low_int = low;
    rez = high_int + low_int;
	rez *= minus;
	rez /= 1575.38;
	return rez;
}

float abs (float x)
{
	if (x<0)
		return -x;
	return x;
}

void dinamo(float rez)
{
	int dt=0,t=0,s=0,d=0,j=0, temp=0;
	if(rez<0)
	{
		rs232_send_byte('-');
		rez = abs(rez);
	}
	rez *= 10000.0; 
	temp = (int) rez;

	dt = temp/10000;
    t = (temp%10000)/1000;
    s = (temp%1000)/100;
    d = (temp%100)/10;
    j = temp%10;

	if (dt)
   		rs232_send_byte(48+dt);
	else
		rs232_send_byte(48);
	rs232_send_byte('.');
   	if (t)
   		rs232_send_byte(48+t);
	else
		rs232_send_byte(48);
   	if (s)
   		rs232_send_byte(48+s);
	else
		rs232_send_byte(48);
   	if (d)
   		rs232_send_byte(48+d);
	else
		rs232_send_byte(48);
   	if (j)
   		rs232_send_byte(48+j);	
	else
		rs232_send_byte(48);
}