/*
 * 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
 * (at your option) 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 "H:\NONAL\nonal.h"

void main()
{
   int rvit, lvit;
   int linestate=LINE_OK, lls;
   char llp=L_O, rlp=L_O;
   int ishere=0, washere=0;
   int32 usd=0, usdd=0;

   initpic();

#ifdef DEBUG
   output_high(DBG4);
#endif
   while(!input(JACK));
#ifdef DEBUG
   output_low(DBG4);
#endif

   while(!washere)
   {

#if FLMODE == 0
      if(!input(MMR) && input(MML))
      {
         rvit=PWM_SLW;
         lvit=PWM_FST;
      }
      else if(!input(MML) && input(MMR))
      {
         lvit=PWM_SLW;
         rvit=PWM_FST;
      }
      else
      {
         lvit=PWM_STD;
         rvit=PWM_STD;
      }

#elif FLMODE == 1
      lls=linestate;
      linestate=getlinepos(&llp,&rlp,lls);

#ifdef DEBUG
      OUTDBG(0);
#endif
      switch(linestate)
      {
         case LINE_OTL:
#ifdef DEBUG
            output_high(DBG3);
#endif
            lvit=PWM_SWR;
            rvit=PWM_FTR;
            break;
         case LINE_GNL:
#ifdef DEBUG
            output_high(DBG0);
#endif
            lvit=PWM_SLW;
            rvit=PWM_FST;
            break;
         case LINE_OK:
#ifdef DEBUG
            output_high(DBG1);
#endif
            if(llp==L_O && rlp!=L_O)
            {
               lvit=PWM_SLW;
               rvit=PWM_FST;
            }
            else if(rlp==L_O && llp!=L_O)
            {
               rvit=PWM_SLW;
               lvit=PWM_FST;
            }
            else
            {
               lvit=PWM_STD;
               rvit=PWM_STD;
            }
            break;
         case LINE_GNR:
#ifdef DEBUG
            output_high(DBG2);
#endif
            lvit=PWM_FST;
            rvit=PWM_SLW;
            break;
         case LINE_OTR:
#ifdef DEBUG
            output_high(DBG5);
#endif
            lvit=PWM_FTR;
            rvit=PWM_SWR;
            break;
         case LINE_ERR:
#ifdef DEBUG
            output_high(DBG4);
#endif
            lvit=rvit=PWM_STD;
            break;
         case LINE_X:
            if(lls!=LINE_X)
            {
               if (usd) usd=0; //nouvelle ligne et deja une
               else usd=1; //nouvelle ligne et pas deja une
            }
            lvit=rvit=PWM_STD;
      }

#elif FLMODE == 2
      if((!input(MMR) || !input(MR) || !input(FR)) && (input(MML) || input(ML) || input(FL)))
      {
         rvit=PWM_SLW;
         lvit=PWM_FST;
      }
      else if((!input(MML) || !input(ML) || !input(FL)) && (input(MMR) || input(MR) || input(FR)))
      {
         lvit=PWM_SLW;
         rvit=PWM_FST;
      }
      else
         lvit=rvit=PWM_STD;
#endif

//detection a droite w/ ultrasons
      if(usd && !usdd)
      {
         usd++;
         if(usd>=USDMAX) usd=0;
      }

      if(usdd && input(DDRL))
         usdd=0;
      else if(usd && input(DDRR) || usdd)
      {
         lvit=rvit=PWM_STOP;
         usdd++;
         if(usdd>=USDDMAX)
               usdd=0;
      }
#ifdef DBGUS
      if(input(DDRL)) output_high(DBG3);
      else output_low(DBG3);

      if(input(DDRR)) output_high(DBG4);
      else output_low(DBG4);

      if (linestate==LINE_X) output_high(DBG0);
      else output_low(DBG0);

      if(usd)
         output_high(DBG1);
      else
         output_low(DBG1);

      if(usdd)
         output_high(DBG2);
      else
         output_low(DBG2);
#endif

//barriere
      if(input(BARRIERE) && !ishere)
      {
         ishere=1;
         delay_us(10000); //anti rebond
      }
      else if(!input(BARRIERE) && ishere)
         washere=1;

      RMOT(rvit);
      LMOT(lvit);
   }

   RMOT(PWM_STOP);
   LMOT(PWM_STOP);

   while(1);
}

void initpic()
{
   setup_adc(ADC_CLOCK_DIV_2);
   setup_spi(FALSE);
   setup_psp(PSP_DISABLED);
   setup_counters(RTCC_INTERNAL,RTCC_DIV_2);
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DIV_BY_1,99,1);
   setup_ccp1(CCP_PWM);
   setup_ccp2(CCP_PWM);
   set_pwm1_duty(PWM_STOP);
   set_pwm2_duty(PWM_STOP);
}

#if FLMODE == 1
int getlinepos(char * llp, char * rlp, int lastls)
{
   char lllp, lrlp;
   lllp=*llp;
   lrlp=*rlp;
   *llp=0;
   *rlp=0;

   if(!input(FL)) *llp += L_F;
   if(!input(ML)) *llp += L_M;
   if(!input(MML)) *llp += L_O;

   if(!input(FR)) *rlp += L_F;
   if(!input(MR)) *rlp += L_M;
   if(!input(MMR)) *rlp += L_O;

   if(*llp==0 && *rlp==0)
   {
      if(lllp<=L_O && lrlp<=L_O || lastls==LINE_X)
      {
         *llp=L_O;
         *rlp=L_O;
      }
      else if(lllp>=L_F) *llp=L_F;
      else if(lrlp>=L_F) *rlp=L_F;
      else if(lllp>=L_M) *llp=L_M;
      else if(lrlp>=L_M) *rlp=L_M;
      else if(lllp>=L_O) *llp=L_O;
      else if(lrlp>=L_O) *rlp=L_O;
   }

   if((*llp>=L_O && *rlp>L_O) || *llp>L_O && *rlp>=L_O) return LINE_X;
   if(*llp==L_F) return LINE_OTL;
   if(*rlp==L_F) return LINE_OTR;
   if(*llp==L_M) return LINE_GNL;
   if(*rlp==L_M) return LINE_GNR;
   if(*llp<=L_O || *rlp<=L_O) return LINE_OK;
//never reached
   return LINE_ERR;
}
#endif