/*
* RemoteControl_GC.c
* This file is part of the project "Remote control for CPG-based robots"
*
* Copyright (C) 2010 - Gabriel CUENDET
*
* This program is free software ; you can redistribute it and /or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation ; either version 2 of the License , or
* any later version .
*
* This program is distributed in the hope that it will be useful ,
* but WITHOUT ANY WARRANTY ; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
* GNU General Public License for more details .
*
* You should have received a copy of the GNU General Public License
* along with this program ; if not , write to the Free Software
* Foundation , Inc ., 59 Temple Place , Suite 330 ,
* Boston , MA 02111 -1307 , USA .
*/


#include "RemoteControl_GC.h"
#include "functions.c"

// Variables which store the joystick calibration informations.
// The value of the ADC when the joystick is centered.
int8 calX = 127;
int8 calY = 127;


#int_RTCC
void  RTCC_isr(void) 
{
   float spd;
   float dir;
   
   char text[16];
   
   
   /// Vertical move of the joystick
   set_adc_channel(1);
   delay_us(10);
   spd = ((float)read_adc() - (float)calX) / (float)calX;
   
   
   /// Horizontal move of the joystick
   set_adc_channel(0);
   delay_us(10);
   dir = - ((float)read_adc() - (float)calY) / (float)calY;
   
   
   /// Display speed and direction
   sprintf(text,"S:%1.2f  D:%1.2f  ",spd,dir);
   display(text,0x00);
   
   // In function of the type of robot, the call to the right update_params function is done
   if(current_robot == AMPHIBOT2 || current_robot == AMPHIBOT3PIC)
   {
      // AmphiBot II or AmphiBot III PIC
      update_amphibot(spd,dir);
   }
   
   else if(current_robot == SALAMANDRA1 || current_robot == SALAMANDRA2)
   {  
      // Salamandra robotica or Salamandra robotica II
      update_salamander(spd, dir);
   }
   
   else if(current_robot == AMPHIBOT3ARM)
   {
      // AmphiBot III ARM (Mathieu's firmware)
      update_amphibot_mathieu(spd, dir);
   }
   
   else
   {
      clear_display();
      sprintf(text,"No ROBOT: %u",current_robot);
      display(text,0x00);
   }
      
}

#int_EXT
void  EXT_isr(void) 
{
    int8 i=0;
   /// Debouncing
   do
   {
      delay_ms(1);
      ++i;
   } while(!input(BTN_ENTER) && i<50);
   
   if(i>45)
   {
      press_button(BTN_ENTER);
   }  
}

#int_EXT1
void  EXT1_isr(void) 
{
   int8 i=0;
   /// Debouncing
   do
   {
      delay_ms(1);
      ++i;
   } while(!input(BTN_CANCEL) && i<50);
   
   if(i>45)
   {
      press_button(BTN_CANCEL);
   }
}

#int_RB
void  RB_isr(void) 
{
   if(!input(BTN_PLUS))
   {
      int8 i=0;
      /// Debouncing
      do
      {
         delay_ms(1);
         ++i;
      } while(!input(BTN_PLUS) && i<50);
      
      if(i>45)
      {
         press_button(BTN_PLUS);
      }
      
   }
   
   else if(!input(BTN_MINUS))
   {
      int8 i=0;
      /// Debouncing
      do
      {
         delay_ms(1);
         ++i;
      } while(!input(BTN_MINUS) && i<50);
      
      if(i>45)
      {
         press_button(BTN_MINUS);
      }
   }
   
}



void main()
{
   char text[16];
   int8 value=0;
   int8 i=0;
   float32 batt_volt=0;
   float32 batt_current=0;
   
   
   setup_adc_ports(AN0_TO_AN1|VSS_VDD);
   setup_adc(ADC_CLOCK_INTERNAL|ADC_TAD_MUL_0);
   setup_wdt(WDT_OFF);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_4);     /**< RTCC_DIV_4 for an interruption every 104ms 
                                                     RTCC_DIV_8 for an interruption every 210ms  **/                                                    
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DISABLED,0,1);
   setup_timer_3(T3_DISABLED|T3_DIV_BY_1);
   setup_vref(FALSE);
   
   disable_interrupts(INT_RTCC);
   disable_interrupts(INT_EXT);
   ext_int_edge(0, H_TO_L);
   disable_interrupts(INT_EXT1);
   ext_int_edge(1, H_TO_L);
   disable_interrupts(INT_RB);
   ext_int_edge(INT_RB, H_TO_L);
   enable_interrupts(GLOBAL);
//Setup_Oscillator parameter not selected from Intr Oscillator Config tab

   /***************************************
    * Initialization sequence            *
    ***************************************/
   
   // Time for the radio interface to boot
   output_high(PIN_C1);
   
   init_display();
   strcpy(text,"Initialisation...");
   display(text,0x00);
   
   delay_ms(1000);
   
   if(!sync())
   {
      output_low(PIN_C1);
      delay_ms(200);
      reset_cpu();
   }
   
   output_low(PIN_C1);
   
   // Calibration of the joystick
   // Important : Do not move the joystick while starting the device!
   /// Vertical calibration of the joystick
   set_adc_channel(1);
   delay_us(10);
   calX = read_adc();
   
   /// Horizontal calibration of the joystick
   set_adc_channel(0);
   delay_us(10);
   calY = read_adc();
   
   // We first set the local channel number to 0x00.
   set_reg_b(REG_INTF_CH,0);
   
   previous_state=ERROR;  
   current_state=RC_AUTONOMOUS;
   
   enable_interrupts(INT_RB);
   enable_interrupts(INT_EXT);
   enable_interrupts(INT_EXT1);


   /***************************************
    * Implementation of the state machine *
    ***************************************/ 
   while(1)
   {
      if((current_state != previous_state) ^ ((current_state==MENU) && (display_changed == 0)))
      {
         if(current_state==RC_AUTONOMOUS)
         {
            clear_display();
            strcpy(text,"Select a robot");
            display(text,0x00);
         
            strcpy(text,"Charge");
            display(text,0x40);
         
            // Display the battery level on bar graph
            batt_volt = battery_voltage();
            if(batt_volt < 3000.0)
            {
               strcpy(text,"EMPTY !");
               display(text,0x49);
            }
            else 
            {
            strcpy(text,"_");
               if(batt_volt >= 3100.0)
               {
                  char_display(text,0x49);   
               }
               if(batt_volt > 3250.0)
               {
                  char_display(text,0x4A);
               }
               if(batt_volt > 3400.0)
               {
                  char_display(text,0x4B);
               }
               if(batt_volt > 3550.0)
               {
                  char_display(text,0x4C);
               }
               if(batt_volt > 3700.0)
               {
                  char_display(text,0x4D);
               }
               if(batt_volt > 3850.0)
               {
                  char_display(text,0x4E);
               }
               if(batt_volt > 4000.0)
               {
                  char_display(text,0x4F);
               }
            set_pos_display(0x50);
            }
         
            previous_state=RC_AUTONOMOUS;
         }
         
         
         else if(current_state==MENU)
         {
            output_low(PIN_C0);
            output_low(PIN_C1);
            output_low(PIN_C2);
            
            display_changed=0;
            display_menus();
            
            // Remember the previous state, before the menu
            // previous_state=MENU;
         }
      
      
         else if(current_state==IN_CHARGE)
         {
            clear_display();
            strcpy(text,"In charge:");
            display(text,0x00);
         
            strcpy(text,"CANCEL TO STOP");
            display(text,0x42);
            previous_state=IN_CHARGE;
         }
      
      
         else if(current_state==STOPPED)
         {
            // The robot has established the communication with the RC
            // First we test if it is in a specific mode already, like charging
            // or already started
            if(!get_reg_b(REG_RWL_CH, &value))
            {
               previous_state=RC_AUTONOMOUS;
               strcpy(text,"NO COMM WITH ROB");
               err(text);
            }    
            
            else
            {
               clear_display();
               sprintf(text,"Ready          %u",current_robot);
               display(text,0x00);
         
               strcpy(text,"ENTER TO MENU");
               display(text,0x43);
               previous_state=STOPPED;
            }
         }
      
      
         else if(current_state==STARTED)
         {
            previous_state=STARTED;
                       
            clear_display();
            strcpy(text, "CANCEL TO STOP");
            display(text,0x42);
          
          output_low(PIN_C0);
          output_low(PIN_C1);
          
          if(current_locomotion_mode == SWIMMING)
          {
             output_high(PIN_C0);
          }
          if(turbo == 1)
          {
             output_high(PIN_C1);
          }
         
            enable_interrupts(INT_RTCC);
         }
      
      
         else if(current_state==ERROR)
         {
            output_high(PIN_C2);
            
            // Remember the previous state, before the error
            //previous_state=ERROR;
         }
   

      }
  
      else if(previous_state == RC_AUTONOMOUS && current_state == RC_AUTONOMOUS)
      {
            // Display the battery level on bar graph
            batt_volt = battery_voltage();
            if(batt_volt < 3000.0)
            {
               strcpy(text,"EMPTY !");
               display(text,0x49);
            }
            else 
            {
               strcpy(text," ");
               for(i=0;i<6;++i)
               {
                  display(text,(0x4A + i));
               }
               
               strcpy(text,"_");
               if(batt_volt >= 3100.0)
               {
                  char_display(text,0x49);   
               }
               if(batt_volt > 3250.0)
               {
                  char_display(text,0x4A);
               }
               if(batt_volt > 3400.0)
               {
                  char_display(text,0x4B);
               }
               if(batt_volt > 3550.0)
               {
                  char_display(text,0x4C);
               }
               if(batt_volt > 3700.0)
               {
                  char_display(text,0x4D);
               }
               if(batt_volt > 3850.0)
               {
                  char_display(text,0x4E);
               }
               if(batt_volt > 4000.0)
               {
                  char_display(text,0x4F);
               }
            set_pos_display(0x50);
            }
            delay_ms(200);
            
            
            // Test if the remote control is charging 
            batt_current = battery_current();
            if(batt_current > 50)
            {
               output_low(PIN_C1);
               output_toggle(PIN_C2);
            }
            else if((batt_current > 30) && (batt_current < 51))
            {
               output_low(PIN_C2);
               output_toggle(PIN_C1);
            }
            else if((batt_current > 0) && (batt_current < 31))
            {
               output_low(PIN_C2);
               output_high(PIN_C1);
            }
            else
            {
               output_low(PIN_C1);
               output_low(PIN_C2);
            }
            
      }   
   }
}