#include <htc.h>
#include <stdio.h>

#include "rs232.h"
#include "i2c.h"
#include "svastara.h"

#define ADRESA_MAG (0x3C)

#ifndef _XTAL_FREQ
#define _XTAL_FREQ 4000000
#endif 

__CONFIG (WDTE_OFF & MCLRE_OFF);

void main()
{
	char high, low;
	float x=0, y=0, x_max=0, y_max=0, x_min=0, y_min=0, faktor=1;
	init_PIC();
	start(5);
	rs232_init();

	while(!i2c_init(ADRESA_MAG, 0x00, 0b00111000));											//Inicijalizacija magnetometra na MINIMU-9
	while(!i2c_init(ADRESA_MAG, 0x01, 0b00100000));
	while(!i2c_init(ADRESA_MAG, 0x02, 0b00000000));
	
	rs232_send_byte(0x0C);
	rs232_send_text("PROJEKT: 5. zadatak\rKalibracija magnetskog kompasa.\r");
	
	while(RA3==1);
	__delay_ms(1000);
	rs232_send_byte(0x0C);	
	
	while(1)
	{
		if (RA3 == 0)																		//Ako je gumb pritisnut predji u mod kalibracije
		{
			x_max=-2; y_max=-2; x_min=2; y_min=2; faktor=0;
			PORTC = 0x02;
			rs232_send_text("Calibration mode...\r");
			__delay_ms(500);
			for (int i = 0; i<100; i++)
			{
				low = read_i2c (ADRESA_MAG, 0x04);
				high = read_i2c (ADRESA_MAG, 0x03);
				x = rezultat (high, low);													//Ocitani podatak osi x u float-u sa magnetometra

				low = read_i2c (ADRESA_MAG, 0x08);
				high = read_i2c (ADRESA_MAG, 0x07);
				y = rezultat (high, low);													//Ocitani podatak osi y u float-u sa magnetometra

				low = read_i2c (ADRESA_MAG, 0x06);											//Citanje podataka z osi ( hardwareski mus? )
				high = read_i2c (ADRESA_MAG, 0x05);

/**************************************************************************/
/***********	Ispisi maximumam i minimuma x i y, x ofseta,	***********/
/***********	y ofseta i faktora prilagodbe, nisu potrebni.	***********/
/**************************************************************************/

				__delay_ms(10);																//Ispis i proracun trenutnog x_max-a
				if (x>x_max)
				x_max = x;
				dinamo (x_max);
				rs232_send_text(" ");

				__delay_ms(10);																//Ispis i proracun trenutnog x_min-a
				if (x<x_min)
				x_min = x;
				dinamo (x_min);
				rs232_send_text(" ");

				__delay_ms(10);																//Ispis i proracun trenutnog y_max-a
				if (y>y_max)
				y_max = y;
				dinamo (y_max);
				rs232_send_text(" ");

				__delay_ms(10);																//Ispis i proracun trenutnog y_min-a
				if (y<y_min)
				y_min = y;
				dinamo (y_min);
				rs232_send_text(" \r");

				__delay_ms(10);
			}
			x = abs (x_max - x_min);														//Duljina 2a elipse
			y = abs (y_max - y_min);														//Duljina 2b elipse

			dinamo (x);
			rs232_send_text(" = x abs\r");													//Ispis duljine 2a elipse
			dinamo (y);
			rs232_send_text(" = y abs\r");													//Ispis duljine 2b elipse

			faktor = x/y;																	//Proracun faktora prilagodbe za y os
			x = x_min + (x_max - x_min)/2.0;												//Proracun pomaka centra elipse po x osi
			y = y_min + (y_max - y_min)/2.0;												//Proracun pomaka centra elipse po y osi	
			start (2);
			dinamo (faktor);																//Ispis faktora prilagodbe, ofseta po x i y osi
			rs232_send_text(" = faktor\r");
			dinamo (x);
			rs232_send_text(" = x ofset\r");
			dinamo (y);
			rs232_send_text(" = y ofset\r");
			rs232_send_text("************************************\r");
			__delay_ms(1000);
		}

/**************************************************************************/		
/***************	Citanje i ispis sve 3 osi magnetometra	***************/
/**************************************************************************/

		PORTC = 0x01;
		low = read_i2c (ADRESA_MAG, 0x04);													//Citanje podataka x osi sa MINIMU-a
		high = read_i2c (ADRESA_MAG, 0x03);
	//	rs232_send_text("Mag x-os: ");
		x_max = rezultat(high, low);														//Preracunavanje u float i kompenziranje za x ofset
		x_max -= x;
		dinamo (x_max);		
	//	rs232_send_text(" gauss\r");

		rs232_send_byte(' ');

		low = read_i2c (ADRESA_MAG, 0x08);													//Citanje podataka y osi sa MINIMU-a
		high = read_i2c (ADRESA_MAG, 0x07);
	//	rs232_send_text("Mag y-os: ");
		y_max = rezultat(high, low);														//Preracunavanje u float i kompenziranje za y ofset
		y_max -= y;
		y_max *= faktor;																	//Kompenziranje za faktor prilagodbe ofset
		dinamo (y_max);
	//	rs232_send_text(" gauss\r");

		low = read_i2c (ADRESA_MAG, 0x06);													//Citanje podataka z osi sa MINIMU-a
		high = read_i2c (ADRESA_MAG, 0x05);

		rs232_send_byte('\r');
		__delay_ms(100);
		PORTC = 0x00;
	//	rs232_send_byte(0x0C);

	}
}
