//---------------------------------------------------------------------------
//  Copyright (c) Jim Wright 2004
//
//  JimMarkovRobot.cpp
//  This is the working class of this application.
//  This is the class where I'm learning about Markov Chains
//
//---------------------------------------------------------------------------

#include "StdAfx.h"
#include "jimmarkovrobot.h"

#define NORMLISTSIZE 9
#define MIDNORMLISTSIZE NORMLISTSIZE/2

const double MaxZDist = 1000.0;
const int    PercentageScale = 30;

/*
** Two math functions that are needed for the math
*/

double StandardDeviation(double* x,double xbar, int n)
{
  double total=0.0;
  double avesquare;
  int i;

  /*
  ** Sum up all of the squares of the entries
  */ 
  for(i=0;i<n;i++)
  {
    total += pow((x[i]-xbar),2);
  }

  /*
  ** Find the Average for the squares
  */ 
  avesquare = (1.0/((double)n-1.0))*total;

  /*
  ** Return the square root of this average.
  */
  return sqrt(avesquare);
}

/*
** This function was found on the internet, I need to go and find the source of this.  
** It's an implementation of the Standard Normal table.
*/

double F(double z)
{
  double t;
  double p;

  t = (z > 0) ? z : (-z);
  p = 1 - pow((1+   t*(0.049867347  + 
                    t*(0.0211410061 + 
                    t*(0.0032776263 + 
                    t*(0.0000380036 + 
                    t*(0.0000488906 + 
                    t*(0.000005383))))))), -16) / 2;
  t = (z > 0) ? p : 1-p;
  return t;
}


/*
** Basic constructor
*/

CJimMarkovRobot::CJimMarkovRobot(void)
: CJimPage()
, m_PercentPage(20,300,362,30,0)
{
  this->m_theta=0;
}

/*
** Basic destructor
*/

CJimMarkovRobot::~CJimMarkovRobot(void)
{
  this->m_theta=0;
}


CJimMarkovRobot::CJimMarkovRobot(int locx,int locy,int width, int height)
: CJimPage(locx,locy,width,height,10)
, m_PercentPage(20,300,362,30,0)
{
  SensorDataMap sensorProbs;
  int i;
  this->m_theta=0;
  for(i=0;i<360;i++)
  {
    sensorProbs[i] = 1.0;
  }
  m_sensorProb.push_back(sensorProbs);
}

void CJimMarkovRobot::SetLocation(CJimPoint loc)
{
  m_location = loc;
  m_objects.push_back(new CJimPoint(loc.GetX(),loc.GetY(),1,RGB(255,0,0)));
}

/*
** Basically the paint the page method, it will run through all of the
** objects that are in the page and draw them.  It will draw the walls
** from the room and it will draw the percentage page.
*/

void CJimMarkovRobot::DrawPage(CDC* hDC)
{
  CJimPage::DrawPage(hDC);
  m_PercentPage.DrawPage(hDC);
}

/*
** This will add a wall to both the Page Object as well as the list for
** the walls the robot can see.
*/

void CJimMarkovRobot::AddWall(CJimLine* object)
{
  m_objects.push_back(object);
  m_walls.push_back(object);
}

/*
** This will add a wall to both the Page Object as well as the list for
** the walls the robot can see.
*/

void CJimMarkovRobot::RunWallData(void)
{
  int i;
  int j;
  m_walldata.clear();

  CJimLine IntersectLine;
  for(i=0;i<360;i++)
  {
    CJimPoint intPoint;
    IntersectLine = CJimLine(m_location,i,MaxZDist);
    for(j=0;j<(int)m_walls.size();j++)
    {
      intPoint = m_walls[j]->Intersect(IntersectLine);

      if((true == m_walls[j]->PointOnLine(intPoint))&&
         (true == IntersectLine.PointOnLine(intPoint)))
      {
        CJimLine testLine(m_location,intPoint);
        double length = testLine.Length();
        m_walldata[i] = length;
//        TRACE("%4d  %9.4f\n",i,length);
        break;
      }
    }
  }
}

/*
** Displays the walls and the lines from the robot
** to the walls for debugging
*/

void CJimMarkovRobot::ShowWallData(CDC* hDC)
{
  int i;
  CJimLine IntersectLine;
  for(i=0;i<360;i++)
  {
    IntersectLine = CJimLine(m_location,i,m_walldata[i]);
    IntersectLine.Draw(hDC,m_locationx+10,m_locationy+10,m_scale);
    m_location.Draw(hDC,m_locationx+10,m_locationy+10,m_scale);
    Sleep(10);
  }
  for(i=0;i<360;i++)
  {
    IntersectLine = CJimLine(m_location,i,m_walldata[i],RGB(255,255,255));
    IntersectLine.Draw(hDC,m_locationx+10,m_locationy+10,m_scale);
    m_location.Draw(hDC,m_locationx+10,m_locationy+10,m_scale);
    Sleep(10);
  }
}

/*
** Sets and draws the theta line for the robot.
** First this erases the line
*/

void CJimMarkovRobot::SetTheta(CDC* hDC, double theta)
{
  CJimLine IntersectLine;

  IntersectLine = CJimLine(m_location,(int)this->m_theta,m_walldata[(int)this->m_theta],RGB(255,255,255));
  IntersectLine.Draw(hDC,m_locationx+10,m_locationy+10,m_scale);
  m_location.Draw(hDC,m_locationx+10,m_locationy+10,m_scale);
  this->m_theta = theta;
  IntersectLine = CJimLine(m_location,(int)this->m_theta,m_walldata[(int)this->m_theta],RGB(255,0,0));
  IntersectLine.Draw(hDC,m_locationx+10,m_locationy+10,m_scale);
}

/*
** Returns the reading from the distance sensor
*/

double CJimMarkovRobot::GetSensorData(double angle)
{
  return m_walldata[(int)angle];
}

/*
** Runs my tests of my Markov chains (This method is the one I'm currently working on)
*/

void CJimMarkovRobot::RunMarkovSensor(CDC* hDC,double sesnorreading,COLORREF color)
{
  int i,j;
  int max;
  double data[NORMLISTSIZE];
  double mean=0.0;
  double stdev;
  int index;
  SensorDataMap sensorProbs;
  SensorDataMap oldSensorProbs;
  double f;

  max = (int)m_sensorProb.size()-1;
  oldSensorProbs = m_sensorProb[max];
  for(i=0;i<360;i++)
  {
    /*
    ** Build a small normalized list of values for this sensors reading
    */
    for(j=0;j<NORMLISTSIZE;j++)
    {
      if(((i-MIDNORMLISTSIZE)+j)<0)
      {
        index = 360+((i-MIDNORMLISTSIZE)+j);
      }
      else
      {
        index = ((i-MIDNORMLISTSIZE)+j)%360;
      }
      data[j] = m_walldata[index];
      mean += data[j];
    }

    /*
    ** Find the probablity of the robot pointing this direction from the sensor
    */
    mean = mean/NORMLISTSIZE;
    stdev = StandardDeviation(data,mean,NORMLISTSIZE);
    f = (sesnorreading-mean)/stdev;
    if(f>0.0)
    {
      f = -1.0*f;
    }
    f = F(f)*2;
    f = f*oldSensorProbs[i];
    sensorProbs[i] = f;
    m_PercentPage.AddObject(new CJimLine(CJimPoint(i+1,PercentageScale),CJimPoint(i+1,PercentageScale-(f*PercentageScale)),color));
  }  

  /*
  ** Put this set of probs on the list.
  */
  m_sensorProb.push_back(sensorProbs);
  m_PercentPage.DrawPage(hDC);
}

/*
** Runs my code to move the robot (this needs a lot more work).
*/

void CJimMarkovRobot::RunMarkovEncoder(CDC* hDC,double encoderReading,COLORREF color)
{
  int i;
  int max;
  int index;
  SensorDataMap sensorProbs;
  SensorDataMap oldSensorProbs;

  /*
  ** Just move the sensors over
  */
  max = (int)m_sensorProb.size()-1;
  oldSensorProbs = m_sensorProb[max];
  for(i=0;i<360;i++)
  {
    if((i+(int)encoderReading)<0)
    {
      index = 360+(i+(int)encoderReading);
    }
    else
    {
      index = (i+(int)encoderReading)%360;
    }
    sensorProbs[i] = oldSensorProbs[index];
    m_PercentPage.AddObject(new CJimLine(CJimPoint(i+1,PercentageScale),CJimPoint(i+1,PercentageScale-(sensorProbs[i]*PercentageScale)),color));
  }  
  m_sensorProb.push_back(sensorProbs);
  m_PercentPage.DrawPage(hDC);
}

/*
** Return the theta for the robot.
*/
double CJimMarkovRobot::GetTheta(void)
{
  return m_theta;
}