#include <OS.h>
#include <stdio.h>
#include <math.h>

#define DELTA_T 0.1
#define NO_OF_NEURONS 6

#define ZERO 1

struct S_Neuron {
  float activation;
  float bias;
  float timeConstant;
  float input;
  float output;
} Neuron;

int main() {
  struct S_Neuron neurons[ NO_OF_NEURONS ];
  float weights[ NO_OF_NEURONS ][ NO_OF_NEURONS ];
  int n, to, from;
  int hours, minutes, seconds, tenths, lastTenths = -1;
  int motor1ID, motor2ID;
  int currentNode;
  unsigned long tenthsSecond;
  float rawLight1, rawLight2;

#ifdef ZERO
  motor1ID = addMotorOnPort( MOTOR_PORT_01 );
  motor2ID = addMotorOnPort( MOTOR_PORT_23 );
#endif

  memset( neurons, 0, sizeof( neurons ) );
  memset( weights, 0, sizeof( weights ) );

  /**
   * Now we initialise all the neurons
   */
#ifdef ZERO
  neurons[ 0 ].bias = -1.0;
  neurons[ 1 ].bias = -1.0;
  neurons[ 2 ].bias = -6.0;
  neurons[ 3 ].bias = -10.0;
  neurons[ 4 ].bias = -10.0;
  neurons[ 5 ].bias = -3.0;
  
  neurons[ 0 ].timeConstant = 0.5;
  neurons[ 1 ].timeConstant = 0.5;
  neurons[ 2 ].timeConstant = 3.0;
  neurons[ 3 ].timeConstant = 0.2;
  neurons[ 4 ].timeConstant = 0.2;
  neurons[ 5 ].timeConstant = 0.5;

  neurons[ 0 ].activation = 1.0;
  neurons[ 1 ].activation = 1.0;
  neurons[ 2 ].activation = 6.0;
  neurons[ 3 ].activation = 10.0;
  neurons[ 4 ].activation = 10.0;
  neurons[ 5 ].activation = 3.0;

  weights[ 0 ][ 2 ] = 1.5;
  weights[ 1 ][ 2 ] = 1.5;
  weights[ 2 ][ 2 ] =12.0;
  weights[ 1 ][ 3 ] = 4.0;
  weights[ 2 ][ 3 ] = 4.0;
  weights[ 3 ][ 3 ] = 7.0;
  weights[ 0 ][ 4 ] = 4.0;
  weights[ 2 ][ 4 ] = 4.0;
  weights[ 4 ][ 4 ] = 7.0;
#else
  neurons[ 0 ].bias = 0.0;
  neurons[ 1 ].bias = 0.0;
  neurons[ 2 ].bias = 0.0;
  neurons[ 3 ].bias = 0.0;
  neurons[ 4 ].bias = 0.0;
  
  neurons[ 0 ].timeConstant = 5.0;
  neurons[ 1 ].timeConstant = 5.0;
  neurons[ 2 ].timeConstant = 5.0;
  neurons[ 3 ].timeConstant = 5.0;
  neurons[ 4 ].timeConstant = 5.0;

  neurons[ 0 ].activation = 0.0;
  neurons[ 1 ].activation = 0.0;
  neurons[ 2 ].activation = 0.0;
  neurons[ 3 ].activation = 0.0;
  neurons[ 4 ].activation = 0.0;

  weights[ 0 ][ 2 ] =  0.5;
  weights[ 1 ][ 2 ] =  0.5;
  weights[ 0 ][ 3 ] =  1.0;
  weights[ 1 ][ 3 ] =  1.0;
  weights[ 2 ][ 3 ] = -3.0;
#endif

  for ( n = 0; n < NO_OF_NEURONS; n++ ) {
    neurons[ n ].input = 0.0;
  }

  for ( tenthsSecond = 0L; ; tenthsSecond++ ) {
    setMotorPower( motor1ID, 
                   neurons[ 3 ].output );
    setMotorPower( motor2ID, 
                   neurons[ 4 ].output );

    rawLight1 = 1.0f - (float)readAD( 0 ) / 255.0;
    rawLight2 = 1.0f - (float)readAD( 1 ) / 255.0;

    neurons[ 0 ].output = ( rawLight1 - 0.15 ) * 1.3f;
    neurons[ 1 ].output = ( rawLight2 - 0.15 ) * 1.3f;

    /*fprintf( stderr, "Going into network: %.3f %.3f neuron2=%.3f\n",
             neurons[ 0 ].output, neurons[ 1 ].output,
             neurons[ 2 ].output );*/
   
    if ( neurons[ 0 ].output < 0.0 ) {
      neurons[ 0 ].output = 0.0;
    }

    if ( neurons[ 1 ].output < 0.0 ) {
      neurons[ 1 ].output = 0.0;
    }

    /*fprintf( stderr, "light activations: %f %f (%f %f)\n",
             neurons[ 0 ].output, neurons[ 1 ].output,
             rawLight1, rawLight2 );*/

    /**
     * We have to do the network layer by layer...
     */
    currentNode = 2;
    for ( to = 2; to < NO_OF_NEURONS; to++ ) {
      for ( from = 0; from < NO_OF_NEURONS; from++ ) {
        if ( currentNode != to ) {
          /*fprintf( stderr, "(%d: added %f+=(%f-%f)*%f/%f=%f)\n",
                   currentNode, 
                   neurons[ currentNode ].activation,
                   neurons[ currentNode ].input,
                   neurons[ currentNode ].activation,
                   DELTA_T,
                   neurons[ currentNode ].timeConstant,
            ( neurons[ currentNode ].input - 
              neurons[ currentNode ].activation ) * DELTA_T /
              neurons[ currentNode ].timeConstant );*/
          
          neurons[ currentNode ].activation += 
            ( neurons[ currentNode ].input - 
              neurons[ currentNode ].activation ) * DELTA_T /
              neurons[ currentNode ].timeConstant;
 
          /*fprintf( stderr, "Before: %d - inp=%f,act=%f,out=%f\n",
                   currentNode, 
                   neurons[ currentNode ].input,
                   neurons[ currentNode ].activation,
                   neurons[ currentNode ].output );*/

          neurons[ currentNode ].output = 
            1.0 / ( 1.0 + 
                    exp( -neurons[ currentNode ].activation -
                    neurons[ currentNode ].bias ) );

          /*neurons[ currentNode ].output = 
            tanh( neurons[ currentNode ].bias -
                  neurons[ currentNode ].activation );*/

          /*fprintf( stderr, "Done %d - inp=%f,act=%f,out=%f\n",
                   currentNode, 
                   neurons[ currentNode ].input,
                   neurons[ currentNode ].activation,
                   neurons[ currentNode ].output );*/

          currentNode = to;
        }

        neurons[ to ].input += neurons[ from ].output * weights[ from ][ to ];
      
        /*fprintf( stderr, "Doing %d to %d - input = %f\n",
                 from, to, neurons[ to ].input );*/

        
      }
    }
 
    for ( n = 2; n < NO_OF_NEURONS; n++ ) {
      neurons[ n ].input = 0.0;
    }

    /*fprintf( stderr, "motor 1: %f: (%f,%f)\n",
             (neurons[ 3 ].output ),
             neurons[0].output,neurons[1].output);

    fprintf( stderr, "motor 2: %f: (%f,%f)\n",
             (neurons[ 4 ].output),
             neurons[0].output,neurons[1].output);*/

    while ( tenths == lastTenths ) {
      getTime( &hours, &minutes, &seconds, &tenths );
    }
    lastTenths = tenths;
  }

  return 0;
}