#include <stdio.h>
#include <util/delay.h>
#include <homelab/adc.h>
#include <homelab/module/lcd_gfx.h>
#include <homelab/module/segment_display.h>
#include <homelab/module/motors.h>
#include <homelab/module/encoders.h>
#include <homelab/module/sensors.h>
#include "homelab/common.h"
#include "homelab/pin.h"
 
#define ULTRASONIC_SPEED_OF_SOUND    33000 // cm/s
 
 
 
/////////////////////////   Peaprogramm  /////////////////////////////
 
int main(void)
{
    char adc0[16],adc1[16],adc2[16],caption0[16],caption1[16];
    unsigned char new_value=0, old_value = 1,new_value0=0, old_value0 = 1,new_value2=0, old_value2 = 1,i,lcd_taust=0;
    int servopos=1, dcpos=1,nupp=0,nupp1=0,nupp2=0,loendur=0, dcpulse=0; 
 
///////////////////////   Init   ///////////////////
 
 //bipolar_init();
    lcd_gfx_init();
    lcd_gfx_clear();
    adc_init(ADC_REF_AVCC, ADC_PRESCALE_8);
    segment_display_init();
    pin leds[3]    = { PIN(C, 3), PIN(C, 4), PIN(C, 5) };
    pin buttons[3] = { PIN(C, 0), PIN(C, 1), PIN(C, 2) };
    pin led_red = PIN(C, 5);
    pin sensors = PIN(G, 0);
    //pin srf05 = PIN(F,2);
 
    pin_setup_output(sensors);
 
    for (i = 0; i < 3; i++)
    {
        pin_setup_output(leds[i]);
        pin_setup_input(buttons[i]);
		dcmotor_init(i);
    }
	dcmotor_init(3);
	encoder_init(0);
	encoder_init(1);
///////////////////////////  Programmi tsükkel //////////////////////////////// 
    while (true)
    {
        new_value0 = pin_get_value(buttons[0]);
        if ((new_value0) && (!old_value0))
        {
            //nupp = PINC & 0x07;
            nupp++;
            if (nupp>4) nupp=0;
            pin_toggle(leds[0]);
			lcd_gfx_clear();
        }
        old_value0 = new_value0;
 
        //sprintf(t,"%d",nupp);
 
        if (nupp==0){
            sprintf(caption0, "   Robotic     ");
            sprintf(caption1, "  HomeLab v5   ");
            sprintf(adc0, "S1 MODULES   ");
            sprintf(adc1, "S2 DEVICES   ");
            sprintf(adc2, "S3 FUNCTIONS ");
        }
////////////////////////// Kasutajaliidese moodul /////////////////////////////////////////////////////
 
        else if (nupp==1){
                new_value = pin_get_value(buttons[1]);
                if ((new_value) && (!old_value))
                {
                    //bit_invert(nupp1,0);
					nupp1=1;
                    lcd_gfx_clear();
                }
                old_value = new_value;
 
                new_value2 = pin_get_value(buttons[2]);
                if ((new_value2) && (!old_value2))
                {
                    //bit_invert(nupp2,0);
                    lcd_gfx_clear();
					nupp1=2;
                }
                old_value2 = new_value2;
 
                if (nupp1==1) {
					pin_clear(leds[1]);
					pin_set(leds[2]);
					lcd_taust=1;
                    sprintf(caption1, "       S2   ");
                }
                else if (nupp1==2) {
					pin_set(leds[1]);
					pin_clear(leds[2]);
					lcd_taust=0;
                    sprintf(caption1, "       S3   ");
                }
                else {
					pin_set(leds[1]);
					pin_set(leds[2]);
                    sprintf(caption1, "       S1   ");
					nupp1=0;
                }                
				//segment_display_write(loendur++ % 10);
 
				sprintf(adc0, "S1: %d ", pin_get_value(buttons[0]));
                sprintf(adc1, "S2: %d ", pin_get_value(buttons[1]));
                sprintf(adc2, "S3: %d ", pin_get_value(buttons[2]));
				sprintf(caption0, "   UI module  ");
                _delay_ms(100); 
        }
////////////////////////// Andurite moodul /////////////////////////////////////////////////////
 
        else if (nupp==2){
                new_value = pin_get_value(buttons[1]);
                if ((new_value) && (!old_value))
                {
                    bit_invert(nupp1,0);
                    lcd_gfx_clear();
                }
                old_value = new_value;
 
                if (nupp1) {
                    pin_clear(sensors);
                    pin_clear(leds[0]);
                    sprintf(caption1, "   on-board   ");
                    sprintf(adc0, "Sound: %d ADC  ", adc_get_average_value(0, 4));
                    sprintf(adc1, "Light: %d ADC  ", adc_get_average_value(1, 4));
                    sprintf(adc2, "Temp.: %d C    ", thermistor_calculate_celsius(adc_get_average_value(2, 4)));
					//sprintf(adc2, "Temp.: %d ADC    ", adc_get_average_value(2, 4));
                }
                else {
                    pin_set(sensors);
                    pin_set(leds[0]);
                    sprintf(caption1, "   external   ");
                    sprintf(adc0, "IR: %d cm     ", ir_distance_calculate_cm(GP2Y0A21YK, adc_get_average_value(3, 4)) );
                    //sprintf(adc1, "UH: %d cm    ", ultrasonic_measure_srf05(srf05));
                    sprintf(adc1, "UH: %d ADC    ", adc_get_average_value(2, 4));
                    sprintf(adc2, "EX: %d ADC    ", adc_get_average_value(1, 4));
                }
                //segment_display_write(adc_get_average_value(3, 4) * 10 / 1024);
                sprintf(caption0, "Sensor module ");
                _delay_ms(500); 
        }
///////////////////////// Mootorite moodul  /////////////////////////////////////////////////////////7
        else if (nupp==3){
 
                sprintf(caption0, "   Motors    ");
                sprintf(caption1, "   module    ");
                sprintf(adc0, "DC    %d dir   ", dcpulse );
                sprintf(adc1, "Servo %d pos   ", servopos );
                sprintf(adc2, "Step. %d dir   ", dcpos );
 
 
 
                servopos++;
                if (servopos>100) { 
                    servopos=-100;  
                    dcpos=dcpos*-1;  
                    pin_toggle(leds[0]);            
                }
 
                dcmotor_drive(0,dcpos);
				dcmotor_drive(1,dcpos);
				dcmotor_drive(2,dcpos);
				dcmotor_drive(3,dcpos);
        		dcpulse=encoder_get_pulses(0);
                //servomotor_position(0,servopos);
 
 
        }
////////////////////////////// Kommunikatsioonimoodul  ////////////////////////////////////////////////////7
        else if (nupp==4){
            sprintf(caption0, "Communication");
            sprintf(caption1, "   module    ");
            sprintf(adc0, " View others  ");
            sprintf(adc1, "              ");
            sprintf(adc2, "              ");
            //pin_toggle(led_red);
            //sw_delay_ms(500);
        }
 
        else {
            pin_toggle(led_red);
            _delay_ms(500); 
        }
////////////////  OUTPUT to LCD ////////////////////////////////////////////////////////        
 
        //lcd_gfx_goto_char_xy(0, 2);
        //lcd_gfx_write_string(t);
		segment_display_write(nupp);
		lcd_gfx_backlight(lcd_taust);
        lcd_gfx_goto_char_xy(0, 0);
        lcd_gfx_write_string(caption0);
        lcd_gfx_goto_char_xy(0, 1);
        lcd_gfx_write_string(caption1);
        lcd_gfx_goto_char_xy(0, 3);
        lcd_gfx_write_string(adc0);
        lcd_gfx_goto_char_xy(0, 4);
        lcd_gfx_write_string(adc1);
        lcd_gfx_goto_char_xy(0, 5);
        lcd_gfx_write_string(adc2);
 
    }
}