
/* 
 * Thomas Pantzer: net2robot.cc, v0.1b  23.9.97 22:29 
 * Thomas Pantzer: net2robot.cc, v0.4.7  8.6.99 16:32 
 * 
 * 
 * Copyright 1997 by Thomas Pantzer
 * 
 * Permission to use, copy, modify, and distribute this software for noncommercial 
 * use and without fee is hereby granted, provided that the above copyright notice 
 * appear in all copies and that both that copyright notice and this permission 
 * notice appear in supporting documentation, and that the name of the Author not be 
 * used in advertising or publicity pertaining to distribution of the software 
 * without specific, written prior permission. The Author makes no representations about
 * the suitability of this software for any purpose.  It is provided "as is" 
 * without expressed or implied warranty. 
 * 
 * 
 * pantec@aix520.informatik.uni-leipzig.de   (Thomas Pantzer) 
 * 
 */

#include <stdio.h>
#include <string.h>
#include "forms.h"

#include "copyright.h"


//#include "floatsize.h"
#include "net2robot.h"

void t_IO_Neuron::Init(RobotContext *R, int i, double xpos, double ypos, int k, double w, double *speed)
{
  t_Neuron::Init(i,xpos,ypos,k,w);
  Robot = R;
  main_speed = speed;
}

void t_IO_Neuron::WriteOutput(int stamp)
{
      double f;
 
      switch (kind)
        {
         case _SPEED_A:
         case _SPEED_A+1:
//	   fprintf(stderr,"main_speed: 0x%lx, %g\n",(long)main_speed,*main_speed);
           f = (t_Neuron::Activation(stamp) - 0.5) * (*main_speed);
#ifdef DEBUG
           fprintf(stdout,"age=%d, setmotor[%d] = %g\n",stamp,kind-SPEED_A,f); 
#endif              
           Robot->SetMotorSpeed(kind - _SPEED_A, f);
           break;

         case _GRIP_A: 
            break;
        }    
  
  t_Neuron::WriteOutput(stamp); 
}


double t_IO_Neuron::Activation(int stamp)
{
    double __Result__ = 0.0;

    switch (kind)
      {
        case DIST_SENSE:
        case DIST_SENSE+1:
        case DIST_SENSE+2:
        case DIST_SENSE+3:
        case DIST_SENSE+4:
        case DIST_SENSE+5:
        case DIST_SENSE+6:
        case DIST_SENSE+7:
  	  Robot->MsgQueue.request_proximity(); 
          __Result__ = Robot->Ired->Sensor[kind - DIST_SENSE]->Value;
#ifdef DEBUG
//          fprintf(stderr,"read %g from %d\n",__Result__,kind); 
#endif    
          break;
          
        case LIGHT_SENSE:
        case LIGHT_SENSE+1:
        case LIGHT_SENSE+2:
        case LIGHT_SENSE+3:
        case LIGHT_SENSE+4:
        case LIGHT_SENSE+5:
        case LIGHT_SENSE+6:
        case LIGHT_SENSE+7:
  	  Robot->MsgQueue.request_ambient(); 
          __Result__ = Robot->Ambl->Sensor[kind - LIGHT_SENSE]->Value;
          break;

/*      doesn't work
        case ARM_POS:
  	  Robot->MsgQueue.request_armpos(); 
          break;
*/
      	case GRIP_WIDTH:
  	  Robot->MsgQueue.request_gripwidth(); 
	  __Result__ = (double)Robot->gr_width / 255.0;
          break;

      	case GRIP_RESIST:
  	  Robot->MsgQueue.request_resistor(); 
        // blank case
          __Result__ = (double)Robot->ohm / 255.0;
	  break;

      	case GRIP_CONTACT:
  	  Robot->MsgQueue.request_contact(); 
	  __Result__ = (double)(Robot->catched & 1); 
        // blank case
          break;

      	case CAM_INTENSITY:
  	  Robot->MsgQueue.request_brightness(); 
	  __Result__ = (double)Robot->Iris.Value; // / 65535.0;
        // blank case
          break;
/*
      	case CAM_DARK_POS:
  	  Robot->MsgQueue.request_maxlight(); 
        // blank case
          break;

      	case CAM_LIGHT_POS:
  	  Robot->MsgQueue.request_minlight(); 
        // blank case
          break;
*/
       }
       
       
   return __Result__;    
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//  t_Net (t_Neuron)
//_______________________________________
void t_RobotNet::Init(RobotContext *r, double *speed)
{
  in_cnt = 0;
  out_cnt = 0;
  age = 0;
  next = NULL;
  actual = NULL;
  root = NULL;
  Robot = r;
  main_speed = speed;
}

void t_RobotNet::Load(const char *s)
{
  int target,px,py;
  float a,w;
  int section = 0;
  char st2[512],*st,tp[64];

#ifdef DEBUG
  fprintf(stdout,"Net Load\n");
#endif

  FILE *F = fopen(s, "r");
  if (F == NULL)
     {
      fprintf(stdout,"file [%s] not found\n",s);
      return;
     } 

  Done();
  
  if (root == NULL)
     {  
      root = new t_Neuron;
      root->Init(0,0,0,ROOT,0);
      in_cnt = 0;
      out_cnt = 0;
      age = 0;
      next = NULL;
     }
     
  actual = root;   
     
  while (fgets(st2, 256, F) != NULL)
    {
     st = &st2[0];
     
     char *TEMP_ = strchr(st, '\n');
     
     while (TEMP_ != NULL)
       {
        *TEMP_ = 0;
     
        if (strcmp(st,"unit definition section :") == 0)
           section = 1;
         else if (strcmp(st,"connection definition section :") == 0)
                 section = 2;
          
            
#ifdef DEBUG
     fprintf(stdout,"(%d) reading line [%s]\n",section,st);        
#endif                          
        
     switch (section)
       {
	 case 1:
  	   if (sscanf(st," %d | | | %g | %g | %s | %d,%d",&target,&a,&w,&tp[0],&px,&py) == 6)
	     {
              while (actual->next != NULL)
	          actual = actual->next;
                  
#ifdef DEBUG
              fprintf(stdout,"reading neuron %d at (%d,%d) weight %g type %c\n",
                                     target,px,py,w,tp[0]);
                     
#endif                                    

#define n_SCALE(x)	(0.1 +(x*0.05))
              t_IO_Neuron *IO_N;
              
	      switch(tp[0])
	        {
	          case 'i': 
                    IO_N = new t_IO_Neuron;
                    IO_N->Init(Robot,target,n_SCALE(px),n_SCALE(py),in_cnt+DIST_SENSE,w,main_speed);
	            
                    actual->next = IO_N;
                    in_cnt++;
	            break;

	          case 's': 
                    actual->next = new t_Neuron;
                    actual->next->Init(target,n_SCALE(px),n_SCALE(py),_SPECIAL_N,w);
	            break;

	          case 'h': 
                    actual->next = new t_Neuron;
                    actual->next->Init(target,n_SCALE(px),n_SCALE(py),_HIDDEN_N,w);
	            break;

	          case 'o': 
                    IO_N = new t_IO_Neuron;
                    IO_N->Init(Robot,target,n_SCALE(px),n_SCALE(py),out_cnt+_SPEED_A,w,main_speed);
	            
                    actual->next = IO_N;
                    out_cnt++;
	            break;
	        }
	    }
            break; // case (section == 1)

	 case 2:
  	   if (sscanf(st," %d | | ",&target) == 1)
              {
               char *s2,big_st[2048];
               int i;

               actual = root->GetNeuron(target);

               strcpy(big_st,st);
               s2 = strchr(big_st,'|');
               s2++;
               s2 = strchr(s2,'|');
               s2++;
               
               i = strlen(s2);
               while (s2[i-1] == ',')
                 {  // line is continued
                  if (fgets(st, 256, F) != NULL)
                     {
                      TEMP_ = strchr(st, '\n');
                      if (TEMP_ != NULL)
                         *TEMP_ = 0;
                      
                      strcat(s2,st);     
                     }
                    else break;  
                  i = strlen(s2);
                 }        

               strcat(s2,", ");
                
               while (strchr(s2,':')!=NULL)
	         {
	          if (sscanf(s2," %d:%g,",&i,&w)==2)
                     {
#ifdef DEBUG
                  fprintf(stdout,"connecting %d -> %d, %g\n",target,i,w);
#endif                 
	              actual->Connect(root->GetNeuron(i),w);

  	              s2 = strchr(s2,',');
	              s2++;
                     } 
	         }
	      }
           break; // case (section == 2)
              
       } // switch (section)

          
        TEMP_++;
        
        if (TEMP_ > &st[256])
           TEMP_ = NULL;
         else
           {   
            st = TEMP_;
            TEMP_ = strchr(st, '\n');          
           } 
       } // while not string end
    }  // while not(eof)

  if (F != NULL)
    fclose(F);
}


t_Neuron *t_RobotNet::GetNeuron(int px, int py)
{
  actual = next;

  if (actual != NULL)
    {
     while (!actual->InBox(px, py) && actual->next != NULL)
       actual = actual->next;

     return (actual->next);
    }
  else
    return NULL;
}


void t_RobotNet::CutSynapse(t_Neuron *a, t_Neuron *b)
{
}


void t_RobotNet::CutNeuron(t_Neuron *a)
{
}


void t_RobotNet::Show()
{
  if ((root != NULL) && (root->next != NULL))
     root->next->Show();
}

void t_RobotNet::Done()
{
  if (root != NULL)
      root->Done();
}

void t_RobotNet::Processing()
{
  age += 1;
  age %= 16;	// max. depth for recurrent networks  		
        	// this is only for backpropagation and update

  if (root != NULL)
     root->WriteOutput(age);
  
  // you have to add here
  //    - check for any errors, mismatches ..
  //	- decide to teach the net
  //    - update the net
}













/**/
/**/
/**/
/**/
/**/
/**/
/**/
/**/
/*****  End Of File  ****************************/
